From: <rt...@us...> - 2009-03-13 07:28:42
|
Revision: 7472 http://playerstage.svn.sourceforge.net/playerstage/?rev=7472&view=rev Author: rtv Date: 2009-03-13 07:28:32 +0000 (Fri, 13 Mar 2009) Log Message: ----------- first webstage demo Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/main.cc code/stage/trunk/worlds/fasr.world Added Paths: ----------- code/stage/trunk/webstage/ code/stage/trunk/webstage/CMakeLists.txt code/stage/trunk/webstage/webstage.cc code/stage/trunk/webstage/world.fed Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-03-13 06:23:03 UTC (rev 7471) +++ code/stage/trunk/CMakeLists.txt 2009-03-13 07:28:32 UTC (rev 7472) @@ -14,8 +14,10 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF) -OPTION (BUILD_GUI "WARNING: turning this off breaks the build right now. Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) +# todo +# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) + cmake_minimum_required( VERSION 2.4 FATAL_ERROR ) IF (CMAKE_MAJOR_VERSION EQUAL 2 AND NOT CMAKE_MINOR_VERSION LESS 6) @@ -79,7 +81,7 @@ MESSAGE( STATUS "BUILD_GUI is ${BUILD_GUI}" ) -IF( BUILD_GUI ) +#IF( BUILD_GUI ) ## the FLTK package script is not useful - it finds an X11-based FLTK on OS X. ## so we can't do this stuff @@ -131,7 +133,7 @@ SET (FLTK_FOUND TRUE) -ENDIF( BUILD_GUI ) +# ENDIF( BUILD_GUI ) #MESSAGE( ${INDENT} "Checking for OpenGL" ) find_package( OpenGL REQUIRED ) @@ -154,6 +156,14 @@ MESSAGE( ${INDENT} "Player not detected. If Player is installed but not detected, check your PKG_CONFIG_PATH." ) ENDIF( PLAYER_FOUND ) +pkg_search_module( WEBSIM websim ) +IF( WEBSIM_FOUND ) + MESSAGE( STATUS ${INDENT} "WebSim version ${WEBSIM_VERSION} detected at ${WEBSIM_PREFIX}" ) + MESSAGE( STATUS " WEBSIM_CFLAGS = ${WEBSIM_CFLAGS}" ) + MESSAGE( STATUS " WEBSIM_LDFLAGS = ${WEBSIM_LDFLAGS}" ) +ELSE( WEBSIM_FOUND ) + MESSAGE( ${INDENT} "WebSim not detected." ) +ENDIF( WEBSIM_FOUND ) ## this should not be necessary now that we do the FLTK config ## carefully above. I'll leave the code here in case of trouble @@ -174,6 +184,7 @@ ${GLIB_INCLUDE_DIRS} ${LIBPNG_INCLUDE_DIRS} ${CMAKE_INCLUDE_PATH} + ${WEBSIM_INCLUDE_DIRS} ) @@ -181,6 +192,7 @@ link_directories(${GLIB_LIBRARY_DIRS} ${GLIB_LIBRARY_DIRS} ${LIBPNG_LIBRARY_DIRS} + ${WEBSIM_LIBRARY_DIRS} ) # work through these subdirs @@ -189,6 +201,10 @@ ADD_SUBDIRECTORY(assets) ADD_SUBDIRECTORY(worlds) +if( WEBSIM_FOUND ) +ADD_SUBDIRECTORY(webstage) +endif( WEBSIM_FOUND ) + IF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND ) ADD_SUBDIRECTORY(libstageplugin) ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND ) Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-03-13 06:23:03 UTC (rev 7471) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-03-13 07:28:32 UTC (rev 7472) @@ -7,7 +7,6 @@ block.cc blockgroup.cc camera.cc - canvas.cc file_manager.cc file_manager.hh gl.cc @@ -27,8 +26,6 @@ model_props.cc model_ranger.cc option.cc - options_dlg.cc - options_dlg.hh powerpack.cc region.cc resource.cc @@ -39,12 +36,21 @@ waypoint.cc world.cc worldfile.cc - worldgui.cc + worldgui.cc + canvas.cc + options_dlg.cc + options_dlg.hh ) # TODO # puck.cc +# todo - build without FLTK - tricky since we use its image loading, etc. +#if( BUILD_GUI ) +# list( APPEND stageSrcs +# # gui-only sources go here +# ) +#endif( BUILD_GUI ) add_library(stage SHARED ${stageSrcs}) Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-03-13 06:23:03 UTC (rev 7471) +++ code/stage/trunk/libstage/main.cc 2009-03-13 07:28:32 UTC (rev 7472) @@ -12,8 +12,10 @@ /* options descriptor */ static struct option longopts[] = { - { "gui", no_argument, NULL, 'g' }, - // { "fast", no_argument, NULL, 'f' }, + { "gui", optional_argument, NULL, 'g' }, + { "port", required_argument, NULL, 'p' }, + { "host", required_argument, NULL, 'h' }, + { "federation", required_argument, NULL, 'f' }, { NULL, 0, NULL, 0 } }; @@ -27,17 +29,19 @@ int ch=0, optindex=0; bool usegui = true; - while ((ch = getopt_long(argc, argv, "gf", longopts, &optindex)) != -1) + while ((ch = getopt_long(argc, argv, "gfp:h:f:", longopts, &optindex)) != -1) { switch( ch ) { case 0: // long option given - printf( "option %s given", longopts[optindex].name ); + printf( "option %s given\n", longopts[optindex].name ); break; case 'g': usegui = false; printf( "[GUI disabled]" ); break; + case 'p': + printf( "PORT %d\n", atoi(optarg) ); case '?': break; default: @@ -47,7 +51,8 @@ puts("");// end the first start-up line - + exit(0); + // arguments at index [optindex] and later are not options, so they // must be world file names Added: code/stage/trunk/webstage/CMakeLists.txt =================================================================== --- code/stage/trunk/webstage/CMakeLists.txt (rev 0) +++ code/stage/trunk/webstage/CMakeLists.txt 2009-03-13 07:28:32 UTC (rev 7472) @@ -0,0 +1,10 @@ +MESSAGE( STATUS "Configuring webstage" ) + +add_executable( webstage webstage.cc ) + +target_link_libraries( webstage ${WEBSIM_LIBRARIES} stage ) + +INSTALL(TARGETS webstage + RUNTIME DESTINATION bin +) + Property changes on: code/stage/trunk/webstage/CMakeLists.txt ___________________________________________________________________ Added: svn:eol-style + native Added: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc (rev 0) +++ code/stage/trunk/webstage/webstage.cc 2009-03-13 07:28:32 UTC (rev 7472) @@ -0,0 +1,219 @@ +#include <getopt.h> +#include <websim.hh> +#include <stage.hh> +using namespace Stg; +#include "config.h" + +/* options descriptor */ +static struct option longopts[] = { + { "gui", no_argument, NULL, 'g' }, + { "port", required_argument, NULL, 'p' }, + { "host", required_argument, NULL, 'h' }, + { "federation", required_argument, NULL, 'f' }, + { NULL, 0, NULL, 0 } +}; + +class WebStage : public websim::WebSim +{ + Stg::World* world; + +public: + WebStage( Stg::World* world, + const std::string& fedfile, + const std::string& host, int port ) : + websim::WebSim( fedfile, host, port ), + world(world) + { + } + + virtual ~WebStage() + {} + + void Push( std::string name ) + { + Stg::Model* mod = world->GetModel( name.c_str() ); + if( mod ) + { + websim::Pose p; + websim::Velocity v; + websim::Acceleration a; + + Stg::Pose sp = mod->GetPose(); + p.x = sp.x; + p.y = sp.y; + p.z = sp.z; + p.a = sp.a; + + Stg::Velocity sv = mod->GetVelocity(); + v.x = sv.x; + v.y = sv.y; + v.z = sv.z; + v.a = sv.a; + + SetPuppetPVA( name, p, v, a ); + } + else + printf( "Warning: attempt to push PVA for unrecognized model \"%s\"\n", + name.c_str() ); + } + + // Interface to be implemented by simulators + virtual bool CreateModel(const std::string& name, + const std::string& type, + std::string& error) + { + printf( "create model name:%s type:%s\n", name.c_str(), type.c_str() ); + return true; + } + + virtual bool DeleteModel(const std::string& name, + std::string& error) + { + printf( "deletee model name:%s \n", name.c_str() ); + return true; + } + + virtual bool SetModelPVA(const std::string& name, + const websim::Pose& p, + const websim::Velocity& v, + const websim::Acceleration& a, + std::string& error) + { + printf( "set model PVA name:%s\n", name.c_str() ); + + Model* mod = world->GetModel( name.c_str() ); + if( mod ) + { + mod->SetPose( Stg::Pose( p.x, p.y, p.z, p.a )); + mod->SetVelocity( Stg::Velocity( v.x, v.y, v.z, v.a )); + // stage doesn't model acceleration + } + else + printf( "Warning: attempt to set PVA for unrecognized model \"%s\"\n", + name.c_str() ); + + return true; + } + + virtual bool GetModelPVA(const std::string& name, + websim::Pose& p, + websim::Velocity& v, + websim::Acceleration& a, + std::string& error) + { + printf( "get model name:%s\n", name.c_str() ); + + Model* mod = world->GetModel( name.c_str() ); + if( mod ) + { + Stg::Pose sp = mod->GetPose(); + p.x = sp.x; + p.y = sp.y; + p.z = sp.z; + p.a = sp.a; + + Stg::Velocity sv = mod->GetVelocity(); + v.x = sv.x; + v.y = sv.y; + v.z = sv.z; + v.a = sv.a; + } + else + printf( "Warning: attempt to set PVA for unrecognized model \"%s\"\n", + name.c_str() ); + + return true; + } +}; + + +int main( int argc, char** argv ) +{ + // initialize libstage - call this first + Stg::Init( &argc, &argv ); + + printf( "WebStage built on %s %s\n", PROJECT, VERSION ); + + std::string fedfilename = ""; + std::string host = "localhost"; + unsigned short port = 8000; + + int ch=0, optindex=0; + bool usegui = true; + + while ((ch = getopt_long(argc, argv, "gh:p:f:", longopts, &optindex)) != -1) + { + switch( ch ) + { + case 0: // long option given + printf( "option %s given", longopts[optindex].name ); + break; + case 'g': + usegui = false; + printf( "[GUI disabled]" ); + break; + case 'p': + port = atoi(optarg); + break; + case 'h': + host = optarg; + break; + case 'f': + fedfilename = optarg; + break; + case '?': + break; + default: + printf("unhandled option %c\n", ch ); + } + } + + puts("");// end the first start-up line + + const char* worldfilename = argv[optind]; + + printf( "[webstage] %s:%u fed %s world %s\n", + host.c_str(), + port, + fedfilename.c_str(), + worldfilename ); + + //websim::Pose p( 0,0,0,0,0,0 ); + // websim::Velocity v( 0,0,0,0,0,0 ); + // websim::Acceleration a( 0,0,0,0,0,0 ); + + World* world = ( usegui ? + new WorldGui( 400, 300, worldfilename ) : + new World( worldfilename ) ); + world->Load( worldfilename ); + + WebStage mws( world, fedfilename, host, port ); + + if( usegui == true ) + { + //don't close the window once time has finished + while( true ) + { + World::UpdateAll(); + // TODO - push changes + + mws.Push( "monkey" ); + mws.Push( "punky" ); + mws.Push( "chunky" ); + + mws.Update(); + } + } + else + { + //close program once time has completed + bool quit = false; + while( quit == false ) + { + quit = World::UpdateAll(); + // TODO - push changes + // mws.SetPuppetPVA( "monkey", p, v, a ); + mws.Update(); + } + } +} Added: code/stage/trunk/webstage/world.fed =================================================================== --- code/stage/trunk/webstage/world.fed (rev 0) +++ code/stage/trunk/webstage/world.fed 2009-03-13 07:28:32 UTC (rev 7472) @@ -0,0 +1,12 @@ + +[federation] +localhost:8000=master +localhost:8001=slave + +[master] +monkey=slave:pioneer2dx +chunky=slave:pioneer2dx +punky=slave:pioneer2dx + + + Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-13 06:23:03 UTC (rev 7471) +++ code/stage/trunk/worlds/fasr.world 2009-03-13 07:28:32 UTC (rev 7472) @@ -169,3 +169,8 @@ #autorob( pose [3.875 6.533 0 134.717] ) #autorob( pose [3.944 4.674 0 -103.060] ) #autorob( pose [4.634 6.897 0 -103.060] ) + + +autorob( name "monkey" pose [5.418 7.478 0 -163.478] joules 300000 ) +autorob( name "chunky" pose [6.418 7.478 0 -163.478] joules 300000 ) +autorob( name "punky" pose [7.418 7.478 0 -163.478] joules 300000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-13 21:48:34
|
Revision: 7479 http://playerstage.svn.sourceforge.net/playerstage/?rev=7479&view=rev Author: rtv Date: 2009-03-13 21:48:20 +0000 (Fri, 13 Mar 2009) Log Message: ----------- added some exit checking Modified Paths: -------------- code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/webstage/webstage.cc code/stage/trunk/webstage/world.fed code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/libstage/model_getset.cc 2009-03-13 21:48:20 UTC (rev 7479) @@ -235,4 +235,3 @@ CallCallbacks( &this->pose ); } - Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/libstage/model_load.cc 2009-03-13 21:48:20 UTC (rev 7479) @@ -299,6 +299,8 @@ { printf( "Libtool error: %s. Something is wrong with your plugin. Quitting\n", lt_dlerror() ); // report the error from libtool + puts( "libtool error #1" ); + fflush( stdout ); exit(-1); } } @@ -308,6 +310,8 @@ lt_dlerror() ); // report the error from libtool PRINT_ERR1( "Failed to open \"%s\". Check that it can be found by searching the directories in your STAGEPATH environment variable, or the current directory if STAGEPATH is not set.]\n", lib ); + puts( "libtool error #2" ); + fflush( stdout ); exit(-1); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/libstage/stage.hh 2009-03-13 21:48:20 UTC (rev 7479) @@ -869,6 +869,7 @@ /** hint that the world needs to be redrawn if a GUI is attached */ void NeedRedraw(){ dirty = true; }; + Model* CreateModel( Model* parent, const char* typestr ); void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/libstage/world.cc 2009-03-13 21:48:20 UTC (rev 7479) @@ -204,35 +204,23 @@ - -void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ) -{ - int parent_entity = wf->GetEntityParent( entity ); +Model* World::CreateModel( Model* parent, const char* typestr ) +{ + Model* mod = NULL; // new model to return - PRINT_DEBUG2( "wf entity %d parent entity %d\n", - entity, parent_entity ); - - Model *mod, *parent; - - parent = (Model*)g_hash_table_lookup( entitytable, - (gpointer)parent_entity ); - // find the creator function pointer in the hash table. use the // vanilla model if the type is NULL. stg_creator_t creator = NULL; //printf( "creating model of type %s\n", typestr ); - char *typestr = (char*)wf->GetEntityType(entity); - - if( typestr ) // look up the string in the typetable - for( int i=0; i<MODEL_TYPE_COUNT; i++ ) - if( strcmp( typestr, typetable[i].token ) == 0 ) - { - creator = typetable[i].creator; - break; - } - + for( int i=0; i<MODEL_TYPE_COUNT; i++ ) + if( strcmp( typestr, typetable[i].token ) == 0 ) + { + creator = typetable[i].creator; + break; + } + // if we found a creator function, call it if( creator ) { @@ -242,15 +230,34 @@ else { PRINT_ERR1( "Unknown model type %s in world file.", - typestr ); + typestr ); exit( 1 ); } //printf( "created model %s\n", mod->Token() ); + return mod; +} + + +void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ) +{ + int parent_entity = wf->GetEntityParent( entity ); + + PRINT_DEBUG2( "wf entity %d parent entity %d\n", + entity, parent_entity ); + + Model* parent = (Model*)g_hash_table_lookup( entitytable, + (gpointer)parent_entity ); + + char *typestr = (char*)wf->GetEntityType(entity); + assert(typestr); + + Model* mod = CreateModel( parent, typestr ); + // configure the model with properties from the world file mod->Load(wf, entity ); - + // record the model we created for this worlfile entry g_hash_table_insert( entitytable, (gpointer)entity, mod ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-13 21:48:20 UTC (rev 7479) @@ -473,6 +473,7 @@ return; } + puts( "User closed window" ); exit(0); } @@ -540,6 +541,7 @@ bool done = worldGui->closeWindowQuery(); if (done) { + puts( "User exited via menu" ); exit(0); } } Modified: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/webstage/webstage.cc 2009-03-13 21:48:20 UTC (rev 7479) @@ -63,13 +63,22 @@ std::string& error) { printf( "create model name:%s type:%s\n", name.c_str(), type.c_str() ); + + Model* mod = world->CreateModel( NULL, type.c_str() ); // top level models only for now + + // rename the model and store it by the new name + mod->SetToken( name.c_str() ); + world->AddModel( mod ); + + + printf( "done." ); return true; } virtual bool DeleteModel(const std::string& name, std::string& error) { - printf( "deletee model name:%s \n", name.c_str() ); + printf( "delete model name:%s \n", name.c_str() ); return true; } @@ -79,7 +88,7 @@ const websim::Acceleration& a, std::string& error) { - printf( "set model PVA name:%s\n", name.c_str() ); + //printf( "set model PVA name:%s\n", name.c_str() ); Model* mod = world->GetModel( name.c_str() ); if( mod ) @@ -101,7 +110,7 @@ websim::Acceleration& a, std::string& error) { - printf( "get model name:%s\n", name.c_str() ); + //printf( "get model name:%s\n", name.c_str() ); Model* mod = world->GetModel( name.c_str() ); if( mod ) @@ -124,6 +133,13 @@ return true; } + + + static void UpdatePuppetCb( std::string name, WebSim::Puppet* pup, void* arg ) + { + WebStage* ws = (WebStage*)arg; + ws->Push( pup->name ); + } }; @@ -177,46 +193,27 @@ port, fedfilename.c_str(), worldfilename ); - - //websim::Pose p( 0,0,0,0,0,0 ); - // websim::Velocity v( 0,0,0,0,0,0 ); - // websim::Acceleration a( 0,0,0,0,0,0 ); World* world = ( usegui ? new WorldGui( 400, 300, worldfilename ) : new World( worldfilename ) ); world->Load( worldfilename ); - WebStage mws( world, fedfilename, host, port ); + WebStage ws( world, fedfilename, host, port ); - if( usegui == true ) + //close program once time has completed + bool quit = false; + while( ! quit ) { - //don't close the window once time has finished - while( true ) - { - World::UpdateAll(); - - // TODO - for all puppets.... - if( port == 8000 ) - { - mws.Push( "monkey" ); - mws.Push( "punky" ); - mws.Push( "chunky" ); - } + ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); - mws.Update(); - } - } - else - { - //close program once time has completed - bool quit = false; - while( quit == false ) - { - quit = World::UpdateAll(); - // TODO - push changes - // mws.SetPuppetPVA( "monkey", p, v, a ); - mws.Update(); - } + // update Stage + //quit = world->Update(); + world->Update(); + + ws.Update(); } + + printf( "Webstage done.\n" ); + return 0; } Modified: code/stage/trunk/webstage/world.fed =================================================================== --- code/stage/trunk/webstage/world.fed 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/webstage/world.fed 2009-03-13 21:48:20 UTC (rev 7479) @@ -4,9 +4,22 @@ localhost:8001=slave [master] -monkey=slave:pioneer2dx -chunky=slave:pioneer2dx -punky=slave:pioneer2dx +position:0=slave:position +position:1=slave:position +position:2=slave:position +position:3=slave:position +position:4=slave:position +position:5=slave:position +position:6=slave:position +position:7=slave:position +position:8=slave:position +position:9=slave:position +position:10=slave:position +position:11=slave:position +position:12=slave:position +position:13=slave:position +position:14=slave:position +position:15=slave:position Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-13 20:42:17 UTC (rev 7478) +++ code/stage/trunk/worlds/fasr.world 2009-03-13 21:48:20 UTC (rev 7479) @@ -14,7 +14,7 @@ # threads may speed things up here depending on available CPU cores & workload # threadpool 0 - threadpool 0 + threadpool 16 # configure the GUI window @@ -121,40 +121,56 @@ obstacle_return 0 ) -puck( pose [ 1.175 2.283 0 0 ] ) -puck( pose [ 0.875 3.139 0 0 ] ) -puck( pose [ 1.043 2.825 0 0 ] ) -puck( pose [ 1.349 2.734 0 0 ] ) -puck( pose [ 2.625 3.068 0 0 ] ) -puck( pose [ 0.447 2.689 0 0 ] ) -puck( pose [ 0.143 3.308 0 0 ] ) -puck( pose [ 0.334 3.441 0 0 ] ) -puck( pose [ 1.439 3.218 0 0 ] ) -puck( pose [ 0.747 2.741 0 0 ] ) -puck( pose [ 0.955 2.086 0 0 ] ) -puck( pose [ 1.781 2.593 0 0 ] ) -puck( pose [ 1.068 2.476 0 0 ] ) +#puck( pose [ 1.175 2.283 0 0 ] ) +#puck( pose [ 0.875 3.139 0 0 ] ) +#puck( pose [ 1.043 2.825 0 0 ] ) +#puck( pose [ 1.349 2.734 0 0 ] ) +#puck( pose [ 2.625 3.068 0 0 ] ) +#puck( pose [ 0.447 2.689 0 0 ] ) +#puck( pose [ 0.143 3.308 0 0 ] ) +#puck( pose [ 0.334 3.441 0 0 ] ) +#puck( pose [ 1.439 3.218 0 0 ] ) +#puck( pose [ 0.747 2.741 0 0 ] ) +#puck( pose [ 0.955 2.086 0 0 ] ) +#puck( pose [ 1.781 2.593 0 0 ] ) +#puck( pose [ 1.068 2.476 0 0 ] ) -puck( pose [ 0.488 3.190 0 0 ] ) -puck( pose [ 1.708 3.198 0 0 ] ) -puck( pose [ 1.440 2.416 0 0 ] ) -puck( pose [ 1.140 3.045 0 0 ] ) -puck( pose [ 0.682 2.969 0 0 ] ) -puck( pose [ 2.205 3.268 0 0 ] ) -puck( pose [ 1.990 2.312 0 0 ] ) -puck( pose [ 0.646 3.486 0 0 ] ) -puck( pose [ 1.670 2.907 0 0 ] ) -puck( pose [ 2.091 2.830 0 0 ] ) -puck( pose [ -0.103 2.564 0 0 ] ) -puck( pose [ 1.950 3.462 0 0 ] ) -puck( pose [ 2.668 2.674 0 0 ] ) -puck( pose [ 0.549 2.367 0 0 ] ) -puck( pose [ 0.162 2.983 0 0 ] ) -puck( pose [ 1.067 3.367 0 0 ] ) -puck( pose [ 1.412 3.604 0 0 ] ) +#puck( pose [ 0.488 3.190 0 0 ] ) +#puck( pose [ 1.708 3.198 0 0 ] ) +#puck( pose [ 1.440 2.416 0 0 ] ) +#puck( pose [ 1.140 3.045 0 0 ] ) +#puck( pose [ 0.682 2.969 0 0 ] ) +#puck( pose [ 2.205 3.268 0 0 ] ) +#puck( pose [ 1.990 2.312 0 0 ] ) +#puck( pose [ 0.646 3.486 0 0 ] ) +#puck( pose [ 1.670 2.907 0 0 ] ) +#puck( pose [ 2.091 2.830 0 0 ] ) +#puck( pose [ -0.103 2.564 0 0 ] ) +#puck( pose [ 1.950 3.462 0 0 ] ) +#puck( pose [ 2.668 2.674 0 0 ] ) +#puck( pose [ 0.549 2.367 0 0 ] ) +#puck( pose [ 0.162 2.983 0 0 ] ) +#puck( pose [ 1.067 3.367 0 0 ] ) +#puck( pose [ 1.412 3.604 0 0 ] ) autorob( pose [5.418 7.478 0 -163.478] joules 300000 ) +autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) +autorob( pose [5.615 6.185 0 107.666] joules 200000 ) +autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) +autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) +autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) +autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) +autorob( pose [7.394 5.595 0 129.497] joules 400000 ) +autorob( pose [6.468 6.708 0 170.743] joules 100000 ) +autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) +autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) +autorob( pose [4.161 5.544 0 -147.713] joules 400000 ) +autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) +autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) +autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) +autorob( pose [6.362 5.632 0 163.239] joules 400000 ) + #autorob( pose [7.559 4.764 0 -139.066] ) #autorob( pose [5.471 7.446 0 77.301] ) #autorob( pose [7.122 4.175 0 -31.440] ) @@ -170,7 +186,3 @@ #autorob( pose [3.944 4.674 0 -103.060] ) #autorob( pose [4.634 6.897 0 -103.060] ) - -autorob( name "monkey" pose [5.418 7.478 0 -163.478] joules 300000 ) -autorob( name "chunky" pose [6.418 7.478 0 -163.478] joules 300000 ) -autorob( name "punky" pose [7.418 7.478 0 -163.478] joules 300000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-14 01:12:48
|
Revision: 7482 http://playerstage.svn.sourceforge.net/playerstage/?rev=7482&view=rev Author: rtv Date: 2009-03-14 01:12:41 +0000 (Sat, 14 Mar 2009) Log Message: ----------- working on webstage Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/webstage/webstage.cc code/stage/trunk/webstage/world.fed code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-14 01:12:23 UTC (rev 7481) +++ code/stage/trunk/libstage/stage.hh 2009-03-14 01:12:41 UTC (rev 7482) @@ -1382,7 +1382,8 @@ void Start(){ paused = false; }; void Stop(){ paused = true; }; void TogglePause(){ paused = !paused; }; - + bool Paused(){ return( paused ); }; + /** show the window - need to call this if you don't Load(). */ void Show(); Modified: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc 2009-03-14 01:12:23 UTC (rev 7481) +++ code/stage/trunk/webstage/webstage.cc 2009-03-14 01:12:41 UTC (rev 7482) @@ -16,12 +16,11 @@ class WebStage : public websim::WebSim { Stg::World* world; - + public: WebStage( Stg::World* world, - const std::string& fedfile, - const std::string& host, int port ) : - websim::WebSim( fedfile, host, port ), + const std::string& host, const unsigned short port ) : + websim::WebSim( host, port ), world(world) { } @@ -199,19 +198,42 @@ new World( worldfilename ) ); world->Load( worldfilename ); - WebStage ws( world, fedfilename, host, port ); + WebStage ws( world, host, port ); + ws.LoadFederationFile( fedfilename ); + puts( "entering main loop" ); + + ws.Go(); + ws.Wait(); + + puts( "through the sync" ); + + //close program once time has completed bool quit = false; while( ! quit ) { - ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); + // tell my friends to start simulating + ws.Go(); + // todo - loop here to drain libevent's output? + puts( "go done" ); + // update Stage - //quit = world->Update(); world->Update(); - ws.Update(); + puts( "update done" ); + + // todo? check for changes? + // send my updates + ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); + + puts( "pushes done" ); + + // wait for goes from all my friends + ws.Wait(); + + puts( "wait done" ); } printf( "Webstage done.\n" ); Modified: code/stage/trunk/webstage/world.fed =================================================================== --- code/stage/trunk/webstage/world.fed 2009-03-14 01:12:23 UTC (rev 7481) +++ code/stage/trunk/webstage/world.fed 2009-03-14 01:12:41 UTC (rev 7482) @@ -21,5 +21,14 @@ position:14=slave:position position:15=slave:position +#[slave] +#rob0=master:position +#rob1=master:position +#rob2=master:position +#rob3=master:position +#rob4=master:position +#rob5=master:position +#rob6=master:position +#rob7=master:position Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-14 01:12:23 UTC (rev 7481) +++ code/stage/trunk/worlds/fasr.world 2009-03-14 01:12:41 UTC (rev 7482) @@ -7,14 +7,14 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 20 # real-time interval between simulation updates in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 # threads may speed things up here depending on available CPU cores & workload -# threadpool 0 - threadpool 16 + threadpool 0 +# threadpool 16 # configure the GUI window This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-14 20:19:35
|
Revision: 7485 http://playerstage.svn.sourceforge.net/playerstage/?rev=7485&view=rev Author: rtv Date: 2009-03-14 20:19:28 +0000 (Sat, 14 Mar 2009) Log Message: ----------- webstage demo working Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/webstage/webstage.cc code/stage/trunk/webstage/world.fed code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/fasr_slave.world Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-14 20:19:06 UTC (rev 7484) +++ code/stage/trunk/libstage/stage.hh 2009-03-14 20:19:28 UTC (rev 7485) @@ -234,7 +234,7 @@ {/*empty*/} /** default constructor uses default non-zero values */ - Size() : x( 0.1 ), y( 0.1 ), z( 0.1 ) + Size() : x( 0.4 ), y( 0.4 ), z( 1.0 ) {/*empty*/} void Load( Worldfile* wf, int section, const char* keyword ); Modified: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc 2009-03-14 20:19:06 UTC (rev 7484) +++ code/stage/trunk/webstage/webstage.cc 2009-03-14 20:19:28 UTC (rev 7485) @@ -203,37 +203,33 @@ puts( "entering main loop" ); - ws.Go(); - ws.Wait(); +// ws.Go(); +// ws.Wait(); - puts( "through the sync" ); +// puts( "through the sync" ); //close program once time has completed bool quit = false; while( ! quit ) { + // todo? check for changes? + // send my updates + ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); + //puts( "pushes done" ); + // tell my friends to start simulating ws.Go(); - // todo - loop here to drain libevent's output? - - puts( "go done" ); - + + // puts( "go done" ); + // update Stage world->Update(); + //puts( "update done" ); - puts( "update done" ); - - // todo? check for changes? - // send my updates - ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); - - puts( "pushes done" ); - // wait for goes from all my friends ws.Wait(); - - puts( "wait done" ); + //puts( "wait done" ); } printf( "Webstage done.\n" ); Modified: code/stage/trunk/webstage/world.fed =================================================================== --- code/stage/trunk/webstage/world.fed 2009-03-14 20:19:06 UTC (rev 7484) +++ code/stage/trunk/webstage/world.fed 2009-03-14 20:19:28 UTC (rev 7485) @@ -1,34 +1,42 @@ [federation] -localhost:8000=master -localhost:8001=slave +localhost:8000=pete +localhost:8001=dud -[master] -position:0=slave:position -position:1=slave:position -position:2=slave:position -position:3=slave:position -position:4=slave:position -position:5=slave:position -position:6=slave:position -position:7=slave:position -position:8=slave:position -position:9=slave:position -position:10=slave:position -position:11=slave:position -position:12=slave:position -position:13=slave:position -position:14=slave:position -position:15=slave:position +[pete] +position:0=dud:model +position:1=dud:model +position:2=dud:model +position:3=dud:model +position:4=dud:model +position:5=dud:model +position:6=dud:model +position:7=dud:model +position:8=dud:model +position:9=dud:model +position:10=dud:model +position:11=dud:model +position:12=dud:model +position:13=dud:model +position:14=dud:model +position:15=dud:model -#[slave] -#rob0=master:position -#rob1=master:position -#rob2=master:position -#rob3=master:position -#rob4=master:position -#rob5=master:position -#rob6=master:position -#rob7=master:position +[dud] +rob00=pete:model +rob01=pete:model +rob02=pete:model +rob03=pete:model +rob04=pete:model +rob05=pete:model +rob06=pete:model +rob07=pete:model +rob08=pete:model +rob09=pete:model +rob10=pete:model +rob11=pete:model +rob12=pete:model +rob13=pete:model +rob14=pete:model +rob15=pete:model Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-14 20:19:06 UTC (rev 7484) +++ code/stage/trunk/worlds/fasr.world 2009-03-14 20:19:28 UTC (rev 7485) @@ -8,7 +8,7 @@ interval_sim 100 # simulation timestep in milliseconds interval_real 0 # real-time interval between simulation updates in milliseconds -paused 1 +paused 0 resolution 0.02 Modified: code/stage/trunk/worlds/fasr_slave.world =================================================================== --- code/stage/trunk/worlds/fasr_slave.world 2009-03-14 20:19:06 UTC (rev 7484) +++ code/stage/trunk/worlds/fasr_slave.world 2009-03-14 20:19:28 UTC (rev 7485) @@ -7,14 +7,14 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 20 # real-time interval between simulation updates in milliseconds -paused 1 +interval_real 0 # real-time interval between simulation updates in milliseconds +paused 0 resolution 0.02 # threads may speed things up here depending on available CPU cores & workload -# threadpool 0 - threadpool 16 +threadpool 0 +# threadpool 16 # configure the GUI window @@ -22,9 +22,9 @@ ( size [ 788.000 842.000 ] - center [ 0.240 -0.382 ] + center [ -0.354 -0.807 ] rotate [ 0 0 ] - scale 35.648 + scale 38.889 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 361 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 16 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" joules 100000 joules_capacity 400000 @@ -176,13 +176,20 @@ #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [5.944 6.951 0 2.937] ) -autorob( name "rob0" pose [6.800 5.897 0 -103.060] ) -autorob( name "rob1" pose [6.405 5.291 0 -103.060] ) -autorob( name "rob2" pose [5.974 5.725 0 -103.060] ) -autorob( name "rob3" pose [4.151 7.272 0 53.540] ) -autorob( name "rob4" pose [6.545 7.459 0 2.937] ) -autorob( name "rob5" pose [7.237 7.533 0 34.450] ) -autorob( name "rob6" pose [3.875 6.533 0 134.717] ) -autorob( name "rob7" pose [3.944 4.674 0 -103.060] ) -autorob( name "rob8" pose [4.634 6.897 0 -103.060] ) +autorob( name "rob00" pose [-4.954 -5.324 0 -103.060] ) +autorob( name "rob01" pose [-5.770 -4.443 0 -103.060] ) +autorob( name "rob02" pose [-2.217 -5.496 0 -103.060] ) +autorob( name "rob03" pose [-3.928 -4.145 0 53.540] ) +autorob( name "rob04" pose [-3.133 -1.518 0 2.937] ) +autorob( name "rob05" pose [-2.441 -1.444 0 34.450] ) +autorob( name "rob06" pose [-6.560 -2.472 0 134.717] ) +autorob( name "rob07" pose [-3.602 -6.799 0 -103.060] ) +autorob( name "rob08" pose [-2.968 -5.334 0 -103.060] ) +autorob( name "rob09" pose [-4.673 -4.005 0 -103.060] ) +autorob( name "rob10" pose [-0.073 -5.913 0 -103.060] ) +autorob( name "rob11" pose [-1.027 -5.520 0 -103.060] ) +autorob( name "rob12" pose [-6.441 -5.071 0 -103.060] ) +autorob( name "rob13" pose [0.039 -7.091 0 -103.060] ) +autorob( name "rob14" pose [-4.842 -6.586 0 -103.060] ) +autorob( name "rob15" pose [-2.878 -3.080 0 -103.060] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-17 01:12:08
|
Revision: 7504 http://playerstage.svn.sourceforge.net/playerstage/?rev=7504&view=rev Author: rtv Date: 2009-03-17 01:12:04 +0000 (Tue, 17 Mar 2009) Log Message: ----------- fixed simple/wander demo Modified Paths: -------------- code/stage/trunk/examples/ctrl/wander.cc code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/examples/ctrl/wander.cc =================================================================== --- code/stage/trunk/examples/ctrl/wander.cc 2009-03-17 01:10:29 UTC (rev 7503) +++ code/stage/trunk/examples/ctrl/wander.cc 2009-03-17 01:12:04 UTC (rev 7504) @@ -42,7 +42,9 @@ // get the data uint32_t sample_count=0; stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); - assert(scan); + + if( ! scan ) + return 0; bool obstruction = false; bool stop = false; Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-03-17 01:10:29 UTC (rev 7503) +++ code/stage/trunk/worlds/simple.world 2009-03-17 01:12:04 UTC (rev 7504) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 100 # real-time interval between simulation updates in milliseconds +interval_real 50 # real-time interval between simulation updates in milliseconds paused 0 @@ -43,6 +43,6 @@ pose [ 0.892 0.800 0 56.500 ] sicklaser() - #ctrl "wander" + ctrl "wander" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-18 03:00:44
|
Revision: 7529 http://playerstage.svn.sourceforge.net/playerstage/?rev=7529&view=rev Author: rtv Date: 2009-03-18 03:00:08 +0000 (Wed, 18 Mar 2009) Log Message: ----------- added basic time support to websim Modified Paths: -------------- code/stage/trunk/examples/ctrl/wander.cc code/stage/trunk/webstage/webstage.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/wander.cc =================================================================== --- code/stage/trunk/examples/ctrl/wander.cc 2009-03-18 02:58:32 UTC (rev 7528) +++ code/stage/trunk/examples/ctrl/wander.cc 2009-03-18 03:00:08 UTC (rev 7529) @@ -42,7 +42,6 @@ // get the data uint32_t sample_count=0; stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); - if( ! scan ) return 0; Modified: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc 2009-03-18 02:58:32 UTC (rev 7528) +++ code/stage/trunk/webstage/webstage.cc 2009-03-18 03:00:08 UTC (rev 7529) @@ -28,7 +28,7 @@ virtual ~WebStage() {} - void Push( std::string name ) + void Push( const std::string& name ) { Stg::Model* mod = world->GetModel( name.c_str() ); if( mod ) @@ -104,6 +104,7 @@ } virtual bool GetModelPVA(const std::string& name, + websim::Time& t, websim::Pose& p, websim::Velocity& v, websim::Acceleration& a, @@ -111,6 +112,8 @@ { //printf( "get model name:%s\n", name.c_str() ); + t = GetTime(); + Model* mod = world->GetModel( name.c_str() ); if( mod ) { @@ -133,8 +136,17 @@ return true; } + virtual websim::Time GetTime() + { + stg_usec_t stgtime = world->SimTimeNow(); - static void UpdatePuppetCb( std::string name, WebSim::Puppet* pup, void* arg ) + websim::Time t; + t.sec = stgtime / 1e6; + t.usec = stgtime - (t.sec * 1e6); + return t; + } + + static void UpdatePuppetCb( const std::string& name, WebSim::Puppet* pup, void* arg ) { WebStage* ws = (WebStage*)arg; ws->Push( pup->name ); @@ -155,7 +167,8 @@ int ch=0, optindex=0; bool usegui = true; - + bool usefedfile = false; + while ((ch = getopt_long(argc, argv, "gh:p:f:", longopts, &optindex)) != -1) { switch( ch ) @@ -175,6 +188,7 @@ break; case 'f': fedfilename = optarg; + usefedfile = true; break; case '?': break; @@ -199,22 +213,19 @@ world->Load( worldfilename ); WebStage ws( world, host, port ); - ws.LoadFederationFile( fedfilename ); + + if( usefedfile ) + ws.LoadFederationFile( fedfilename ); puts( "entering main loop" ); -// ws.Go(); -// ws.Wait(); - -// puts( "through the sync" ); - - //close program once time has completed bool quit = false; while( ! quit ) { // todo? check for changes? // send my updates + ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); //puts( "pushes done" ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-18 02:58:32 UTC (rev 7528) +++ code/stage/trunk/worlds/fasr.world 2009-03-18 03:00:08 UTC (rev 7529) @@ -13,8 +13,8 @@ resolution 0.02 # threads may speed things up here depending on available CPU cores & workload - threadpool 0 -# threadpool 16 +# threadpool 8 + threadpool 16 # configure the GUI window This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-19 06:39:43
|
Revision: 7532 http://playerstage.svn.sourceforge.net/playerstage/?rev=7532&view=rev Author: rtv Date: 2009-03-19 06:38:54 +0000 (Thu, 19 Mar 2009) Log Message: ----------- fixed localization_origin bug and cleaning up Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/waypoint.cc code/stage/trunk/libstage/world.cc code/stage/trunk/stage.pc.in Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/CMakeLists.txt 2009-03-19 06:38:54 UTC (rev 7532) @@ -14,6 +14,7 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF) +add_definitions(-g -Wall) # todo # OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/libstage/model.cc 2009-03-19 06:38:54 UTC (rev 7532) @@ -678,28 +678,27 @@ // printf( "[%llu] %s update (%d subs)\n", // this->world->sim_time, this->token, this->subs ); - // f we're drawing current and a power pack has been installed + // if we're drawing current and a power pack has been installed PowerPack* pp = FindPowerPack(); if( pp && ( watts > 0 )) - { - // consume energy stored in the power pack - stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - pp->Subtract( consumed ); - - /* - printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", - token, - watts, - consumed, - power_pack, - power_pack->stored, - power_pack->capacity, - power_pack->stored / power_pack->capacity * 100.0 ); - */ - - } - + { + // consume energy stored in the power pack + stg_joules_t consumed = watts * (world->interval_sim * 1e-6); + pp->Subtract( consumed ); + + /* + printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", + token, + watts, + consumed, + power_pack, + power_pack->stored, + power_pack->capacity, + power_pack->stored / power_pack->capacity * 100.0 ); + */ + } + CallCallbacks( &hooks.update ); last_update = world->sim_time; } @@ -720,9 +719,6 @@ return geom.size.z + m_child; } - - - void Model::AddToPose( double dx, double dy, double dz, double da ) { if( dx || dy || dz || da ) @@ -832,9 +828,9 @@ Model* Model::ConditionalMove( Pose newpose ) { - if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) - printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n", - token, newpose.x, newpose.y, newpose.z, newpose.a ); +// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) +// printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n", +// token, newpose.x, newpose.y, newpose.z, newpose.a ); Pose startpose = pose; pose = newpose; // do the move provisionally - we might undo it below @@ -848,11 +844,10 @@ CommitTestedPose(); // shift anyrecursively commit to blocks to the new pose world->dirty = true; // need redraw } - - if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) - printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n", - token, pose.x, pose.y, pose.z, pose.a ); +// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) +// printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n", +// token, pose.x, pose.y, pose.z, pose.a ); return hitmod; } @@ -862,13 +857,13 @@ { if( disabled ) return; - - if( velocity.IsZero() ) - { - PRINT_WARN1( "model %s has velocity zero but its pose is being updated", token ); - return; - } - + +// if( velocity.IsZero() ) +// { +// PRINT_WARN1( "model %s has velocity zero but its pose is being updated", token ); +// return; +// } + // TODO - control this properly, and maybe do it faster //if( 0 ) //if( (world->updates % 10 == 0) ) Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/libstage/model_position.cc 2009-03-19 06:38:54 UTC (rev 7532) @@ -24,52 +24,52 @@ using namespace Stg; /** -@ingroup model -@defgroup model_position Position model + @ingroup model + @defgroup model_position Position model -The position model simulates a -mobile robot base. It can drive in one of two modes; either -<i>differential</i>, i.e. able to control its speed and turn rate by -driving left and roght wheels like a Pioneer robot, or -<i>omnidirectional</i>, i.e. able to control each of its three axes -independently. + The position model simulates a + mobile robot base. It can drive in one of two modes; either + <i>differential</i>, i.e. able to control its speed and turn rate by + driving left and roght wheels like a Pioneer robot, or + <i>omnidirectional</i>, i.e. able to control each of its three axes + independently. -API: Stg::ModelPosition + API: Stg::ModelPosition -<h2>Worldfile properties</h2> + <h2>Worldfile properties</h2> -@par Summary and default values + @par Summary and default values -@verbatim -position -( - # position properties - drive "diff" - localization "gps" - localization_origin [ <defaults to model's start pose> ] + @verbatim + position + ( + # position properties + drive "diff" + localization "gps" + localization_origin [ <defaults to model's start pose> ] - # odometry error model parameters, - # only used if localization is set to "odom" - odom_error [0.03 0.03 0.00 0.05] + # odometry error model parameters, + # only used if localization is set to "odom" + odom_error [0.03 0.03 0.00 0.05] - # model properties -) -@endverbatim + # model properties + ) + @endverbatim -@par Note -Since Stage-1.6.5 the odom property has been removed. Stage will generate a warning if odom is defined in your worldfile. See localization_origin instead. + @par Note + Since Stage-1.6.5 the odom property has been removed. Stage will generate a warning if odom is defined in your worldfile. See localization_origin instead. -@par Details + @par Details -- drive "diff", "omni" or "car"\n - select differential-steer model(like a Pioneer), omnidirectional mode or carlike (velocity and steering angle). -- localization "gps" or "odom"\n - if "gps" the position model reports its position with perfect accuracy. If "odom", a simple odometry model is used and position data drifts from the ground truth over time. The odometry model is parameterized by the odom_error property. -- localization_origin [x y theta] -- set the origin of the localization coordinate system. By default, this is copied from the model's initial pose, so the robot reports its position relative to the place it started out. Tip: If localization_origin is set to [0 0 0] and localization is "gps", the model will return its true global position. This is unrealistic, but useful if you want to abstract away the details of localization. Be prepared to justify the use of this mode in your research! -- odom_error [x y theta] -- parameters for the odometry error model used when specifying localization "odom". Each value is the maximum proportion of error in intergrating x, y, and theta velocities to compute odometric position estimate. For each axis, if the the value specified here is E, the actual proportion is chosen at startup at random in the range -E/2 to +E/2. Note that due to rounding errors, setting these values to zero does NOT give you perfect localization - for that you need to choose localization "gps". - */ + - drive "diff", "omni" or "car"\n + select differential-steer model(like a Pioneer), omnidirectional mode or carlike (velocity and steering angle). + - localization "gps" or "odom"\n + if "gps" the position model reports its position with perfect accuracy. If "odom", a simple odometry model is used and position data drifts from the ground truth over time. The odometry model is parameterized by the odom_error property. + - localization_origin [x y theta] + - set the origin of the localization coordinate system. By default, this is copied from the model's initial pose, so the robot reports its position relative to the place it started out. Tip: If localization_origin is set to [0 0 0] and localization is "gps", the model will return its true global position. This is unrealistic, but useful if you want to abstract away the details of localization. Be prepared to justify the use of this mode in your research! + - odom_error [x y theta] + - parameters for the odometry error model used when specifying localization "odom". Each value is the maximum proportion of error in intergrating x, y, and theta velocities to compute odometric position estimate. For each axis, if the the value specified here is E, the actual proportion is chosen at startup at random in the range -E/2 to +E/2. Note that due to rounding errors, setting these values to zero does NOT give you perfect localization - for that you need to choose localization "gps". +*/ @@ -86,393 +86,389 @@ const stg_position_localization_mode_t POSITION_LOCALIZATION_DEFAULT = STG_POSITION_LOCALIZATION_GPS; const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT = STG_POSITION_DRIVE_DIFFERENTIAL; -//TODO make instance attempt to register an option (as customvisualizations do) Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "", false, NULL ); Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", false, NULL ); - ModelPosition::ModelPosition( World* world, - Model* parent ) + Model* parent ) : Model( world, parent, MODEL_TYPE_POSITION ), - waypoints( NULL ), - waypoint_count( 0 ) + waypoints( NULL ), + waypoint_count( 0 ) { - PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n", - id, typestr ); - - // assert that Update() is reentrant for this derived model - thread_safe = true; + PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n", + id, typestr ); + + // assert that Update() is reentrant for this derived model + thread_safe = true; - // 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 ); - - // configure the position-specific stuff - - // control - memset( &goal, 0, sizeof(goal)); - drive_mode = POSITION_DRIVE_DEFAULT; - control_mode = POSITION_CONTROL_DEFAULT; - - // localization - localization_mode = POSITION_LOCALIZATION_DEFAULT; - - integration_error.x = - drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_X - - STG_POSITION_INTEGRATION_ERROR_MAX_X/2.0; - - integration_error.y = - drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_Y - - STG_POSITION_INTEGRATION_ERROR_MAX_Y/2.0; - - integration_error.a = - drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A - - STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0; - - RegisterOption( &showCoords ); - RegisterOption( &showWaypoints ); + // 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 ); + + // configure the position-specific stuff + + // control + memset( &goal, 0, sizeof(goal)); + drive_mode = POSITION_DRIVE_DEFAULT; + control_mode = POSITION_CONTROL_DEFAULT; + + // localization + localization_mode = POSITION_LOCALIZATION_DEFAULT; + + integration_error.x = + drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_X - + STG_POSITION_INTEGRATION_ERROR_MAX_X/2.0; + + integration_error.y = + drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_Y - + STG_POSITION_INTEGRATION_ERROR_MAX_Y/2.0; + + integration_error.a = + drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A - + STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0; + + RegisterOption( &showCoords ); + RegisterOption( &showWaypoints ); } ModelPosition::~ModelPosition( void ) { - // nothing to do + // nothing to do } void ModelPosition::Load( void ) { - Model::Load(); + Model::Load(); - char* keyword = NULL; + // load steering mode + if( wf->PropertyExists( wf_entity, "drive" ) ) + { + const char* mode_str = + wf->ReadString( wf_entity, "drive", NULL ); - // load steering mode - if( wf->PropertyExists( wf_entity, "drive" ) ) + if( mode_str ) { - const char* mode_str = - wf->ReadString( wf_entity, "drive", NULL ); + if( strcmp( mode_str, "diff" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL; + else if( strcmp( mode_str, "omni" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_OMNI; + else if( strcmp( mode_str, "car" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_CAR; + else + { + PRINT_ERR1( "invalid position drive mode specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.", mode_str ); + } + } + } - if( mode_str ) - { - if( strcmp( mode_str, "diff" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL; - else if( strcmp( mode_str, "omni" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_OMNI; - else if( strcmp( mode_str, "car" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_CAR; - else - { - PRINT_ERR1( "invalid position drive mode specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.", mode_str ); - } - } - } + // load odometry if specified + if( wf->PropertyExists( wf_entity, "odom" ) ) + { + PRINT_WARN1( "the odom property is specified for model \"%s\"," + " but this property is no longer available." + " Use localization_origin instead. See the position" + " entry in the manual or src/model_position.c for details.", + this->token ); + } - // load odometry if specified - if( wf->PropertyExists( wf_entity, "odom" ) ) - { - PRINT_WARN1( "the odom property is specified for model \"%s\"," - " but this property is no longer available." - " Use localization_origin instead. See the position" - " entry in the manual or src/model_position.c for details.", - this->token ); - } + // set the starting pose as my initial odom position. This could be + // overwritten below if the localization_origin property is + // specified + est_origin = this->GetGlobalPose(); - // set the starting pose as my initial odom position. This could be - // overwritten below if the localization_origin property is - // specified - est_origin = this->GetGlobalPose(); + if( wf->PropertyExists( wf_entity, "localization_origin" ) ) + { + est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0, est_origin.x ); + est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1, est_origin.y ); + est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2, est_origin.z ); + est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3, est_origin.a ); - if( wf->PropertyExists( wf_entity, keyword ) ) - { - est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0, est_origin.x ); - est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1, est_origin.y ); - est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2, est_origin.z ); - est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3, est_origin.a ); + // compute our localization pose based on the origin and true pose + Pose gpose = this->GetGlobalPose(); - // compute our localization pose based on the origin and true pose - Pose gpose = this->GetGlobalPose(); + est_pose.a = normalize( gpose.a - est_origin.a ); + double cosa = cos(est_origin.a); + double sina = sin(est_origin.a); + double dx = gpose.x - est_origin.x; + double dy = gpose.y - est_origin.y; + est_pose.x = dx * cosa + dy * sina; + est_pose.y = dy * cosa - dx * sina; - est_pose.a = normalize( gpose.a - est_origin.a ); - double cosa = cos(est_origin.a); - double sina = sin(est_origin.a); - double dx = gpose.x - est_origin.x; - double dy = gpose.y - est_origin.y; - est_pose.x = dx * cosa + dy * sina; - est_pose.y = dy * cosa - dx * sina; + // zero position error: assume we know exactly where we are on startup + memset( &est_pose_error, 0, sizeof(est_pose_error)); + } - // zero position error: assume we know exactly where we are on startup - memset( &est_pose_error, 0, sizeof(est_pose_error)); - } + // odometry model parameters + if( wf->PropertyExists( wf_entity, "odom_error" ) ) + { + integration_error.x = + wf->ReadTupleLength( wf_entity, "odom_error", 0, integration_error.x ); + integration_error.y = + wf->ReadTupleLength( wf_entity, "odom_error", 1, integration_error.y ); + integration_error.z = + wf->ReadTupleLength( wf_entity, "odom_error", 2, integration_error.z ); + integration_error.a + = wf->ReadTupleAngle( wf_entity, "odom_error", 3, integration_error.a ); + } - // odometry model parameters - if( wf->PropertyExists( wf_entity, "odom_error" ) ) - { - integration_error.x = - wf->ReadTupleLength( wf_entity, "odom_error", 0, integration_error.x ); - integration_error.y = - wf->ReadTupleLength( wf_entity, "odom_error", 1, integration_error.y ); - integration_error.z = - wf->ReadTupleLength( wf_entity, "odom_error", 2, integration_error.z ); - integration_error.a - = wf->ReadTupleAngle( wf_entity, "odom_error", 3, integration_error.a ); - } + // choose a localization model + if( wf->PropertyExists( wf_entity, "localization" ) ) + { + const char* loc_str = + wf->ReadString( wf_entity, "localization", NULL ); - // choose a localization model - if( wf->PropertyExists( wf_entity, "localization" ) ) + if( loc_str ) { - const char* loc_str = - wf->ReadString( wf_entity, "localization", NULL ); - - if( loc_str ) - { - if( strcmp( loc_str, "gps" ) == 0 ) - localization_mode = STG_POSITION_LOCALIZATION_GPS; - else if( strcmp( loc_str, "odom" ) == 0 ) - localization_mode = STG_POSITION_LOCALIZATION_ODOM; - else - PRINT_ERR2( "unrecognized localization mode \"%s\" for model \"%s\"." - " Valid choices are \"gps\" and \"odom\".", - loc_str, this->token ); - } - else - PRINT_ERR1( "no localization mode string specified for model \"%s\"", - this->token ); + if( strcmp( loc_str, "gps" ) == 0 ) + localization_mode = STG_POSITION_LOCALIZATION_GPS; + else if( strcmp( loc_str, "odom" ) == 0 ) + localization_mode = STG_POSITION_LOCALIZATION_ODOM; + else + PRINT_ERR2( "unrecognized localization mode \"%s\" for model \"%s\"." + " Valid choices are \"gps\" and \"odom\".", + loc_str, this->token ); } + else + PRINT_ERR1( "no localization mode string specified for model \"%s\"", + this->token ); + } - // TODO - // we've probably poked the localization data, so we must let people know - //model_change( mod, &mod->data ); + // TODO + // we've probably poked the localization data, so we must let people know + //model_change( mod, &mod->data ); } void ModelPosition::Update( void ) { - PRINT_DEBUG1( "[%lu] position update", this->world->sim_time ); + PRINT_DEBUG1( "[%lu] position update", this->world->sim_time ); - // stop by default - Velocity vel; - memset( &vel, 0, sizeof(Velocity) ); + // stop by default + Velocity vel; + memset( &vel, 0, sizeof(Velocity) ); - if( this->subs ) // no driving if noone is subscribed - { - switch( control_mode ) - { - case STG_POSITION_CONTROL_VELOCITY : - { - PRINT_DEBUG( "velocity control mode" ); - PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)", - this->token, - this->goal.x, - this->goal.y, - this->goal.a ); + if( this->subs ) // no driving if noone is subscribed + { + switch( control_mode ) + { + case STG_POSITION_CONTROL_VELOCITY : + { + PRINT_DEBUG( "velocity control mode" ); + PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)", + this->token, + this->goal.x, + this->goal.y, + this->goal.a ); - switch( drive_mode ) - { - case STG_POSITION_DRIVE_DIFFERENTIAL: - // differential-steering model, like a Pioneer - vel.x = goal.x; - vel.y = 0; - vel.a = goal.a; - break; + switch( drive_mode ) + { + case STG_POSITION_DRIVE_DIFFERENTIAL: + // differential-steering model, like a Pioneer + vel.x = goal.x; + vel.y = 0; + vel.a = goal.a; + break; - case STG_POSITION_DRIVE_OMNI: - // direct steering model, like an omnidirectional robot - vel.x = goal.x; - vel.y = goal.y; - vel.a = goal.a; - break; + case STG_POSITION_DRIVE_OMNI: + // direct steering model, like an omnidirectional robot + vel.x = goal.x; + vel.y = goal.y; + vel.a = goal.a; + break; - case STG_POSITION_DRIVE_CAR: - // car like steering model based on speed and turning angle - vel.x = goal.x * cos(goal.a); - vel.y = 0; - vel.a = goal.x * sin(goal.a)/1.0; // here 1.0 is the wheel base, this should be a config option - break; + case STG_POSITION_DRIVE_CAR: + // car like steering model based on speed and turning angle + vel.x = goal.x * cos(goal.a); + vel.y = 0; + vel.a = goal.x * sin(goal.a)/1.0; // here 1.0 is the wheel base, this should be a config option + break; - default: - PRINT_ERR1( "unknown steering mode %d", drive_mode ); - } - } break; + default: + PRINT_ERR1( "unknown steering mode %d", drive_mode ); + } + } break; - case STG_POSITION_CONTROL_POSITION: - { - PRINT_DEBUG( "position control mode" ); + case STG_POSITION_CONTROL_POSITION: + { + PRINT_DEBUG( "position control mode" ); - double x_error = goal.x - est_pose.x; - double y_error = goal.y - est_pose.y; - double a_error = normalize( goal.a - est_pose.a ); + double x_error = goal.x - est_pose.x; + double y_error = goal.y - est_pose.y; + double a_error = normalize( goal.a - est_pose.a ); - PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error ); + PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error ); - // speed limits for controllers - // TODO - have these configurable - double max_speed_x = 0.4; - double max_speed_y = 0.4; - double max_speed_a = 1.0; + // speed limits for controllers + // TODO - have these configurable + double max_speed_x = 0.4; + double max_speed_y = 0.4; + double max_speed_a = 1.0; - switch( drive_mode ) - { - case STG_POSITION_DRIVE_OMNI: - { - // this is easy - we just reduce the errors in each axis - // independently with a proportional controller, speed - // limited - vel.x = MIN( x_error, max_speed_x ); - vel.y = MIN( y_error, max_speed_y ); - vel.a = MIN( a_error, max_speed_a ); - } - break; + switch( drive_mode ) + { + case STG_POSITION_DRIVE_OMNI: + { + // this is easy - we just reduce the errors in each axis + // independently with a proportional controller, speed + // limited + vel.x = MIN( x_error, max_speed_x ); + vel.y = MIN( y_error, max_speed_y ); + vel.a = MIN( a_error, max_speed_a ); + } + break; - case STG_POSITION_DRIVE_DIFFERENTIAL: - { - // axes can not be controlled independently. We have to - // turn towards the desired x,y position, drive there, - // then turn to face the desired angle. this is a - // simple controller that works ok. Could easily be - // improved if anyone needs it better. Who really does - // position control anyhoo? + case STG_POSITION_DRIVE_DIFFERENTIAL: + { + // axes can not be controlled independently. We have to + // turn towards the desired x,y position, drive there, + // then turn to face the desired angle. this is a + // simple controller that works ok. Could easily be + // improved if anyone needs it better. Who really does + // position control anyhoo? - // start out with no velocity - Velocity calc; - memset( &calc, 0, sizeof(calc)); + // start out with no velocity + Velocity calc; + memset( &calc, 0, sizeof(calc)); - double close_enough = 0.02; // fudge factor + double close_enough = 0.02; // fudge factor - // if we're at the right spot - if( fabs(x_error) < close_enough && fabs(y_error) < close_enough ) - { - PRINT_DEBUG( "TURNING ON THE SPOT" ); - // turn on the spot to minimize the error - calc.a = MIN( a_error, max_speed_a ); - calc.a = MAX( a_error, -max_speed_a ); - } - else - { - PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" ); - // turn to face the goal point - double goal_angle = atan2( y_error, x_error ); - double goal_distance = hypot( y_error, x_error ); + // if we're at the right spot + if( fabs(x_error) < close_enough && fabs(y_error) < close_enough ) + { + PRINT_DEBUG( "TURNING ON THE SPOT" ); + // turn on the spot to minimize the error + calc.a = MIN( a_error, max_speed_a ); + calc.a = MAX( a_error, -max_speed_a ); + } + else + { + PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" ); + // turn to face the goal point + double goal_angle = atan2( y_error, x_error ); + double goal_distance = hypot( y_error, x_error ); - a_error = normalize( goal_angle - est_pose.a ); - calc.a = MIN( a_error, max_speed_a ); - calc.a = MAX( a_error, -max_speed_a ); + a_error = normalize( goal_angle - est_pose.a ); + calc.a = MIN( a_error, max_speed_a ); + calc.a = MAX( a_error, -max_speed_a ); - PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error, goal_distance ); + PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error, goal_distance ); - // if we're pointing about the right direction, move - // forward - if( fabs(a_error) < M_PI/16 ) - { - PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" ); - calc.x = MIN( goal_distance, max_speed_x ); - } - } + // if we're pointing about the right direction, move + // forward + if( fabs(a_error) < M_PI/16 ) + { + PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" ); + calc.x = MIN( goal_distance, max_speed_x ); + } + } - // now set the underlying velocities using the normal - // diff-steer model - vel.x = calc.x; - vel.y = 0; - vel.a = calc.a; - } - break; + // now set the underlying velocities using the normal + // diff-steer model + vel.x = calc.x; + vel.y = 0; + vel.a = calc.a; + } + break; - default: - PRINT_ERR1( "unknown steering mode %d", (int)drive_mode ); - } - } - break; + default: + PRINT_ERR1( "unknown steering mode %d", (int)drive_mode ); + } + } + break; - default: - PRINT_ERR1( "unrecognized position command mode %d", control_mode ); - } + default: + PRINT_ERR1( "unrecognized position command mode %d", control_mode ); + } - // simple model of power consumption - watts = STG_POSITION_WATTS + - fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + - fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + - fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; + // simple model of power consumption + watts = STG_POSITION_WATTS + + fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; - //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", - // this->token, - // this->velocity.x, - // this->velocity.y, - // this->velocity.a ); + //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", + // this->token, + // this->velocity.x, + // this->velocity.y, + // this->velocity.a ); - this->SetVelocity( vel ); - } + this->SetVelocity( vel ); + } - switch( localization_mode ) - { - case STG_POSITION_LOCALIZATION_GPS: - { - // compute our localization pose based on the origin and true pose - Pose gpose = this->GetGlobalPose(); + switch( localization_mode ) + { + case STG_POSITION_LOCALIZATION_GPS: + { + // compute our localization pose based on the origin and true pose + Pose gpose = this->GetGlobalPose(); - est_pose.a = normalize( gpose.a - est_origin.a ); - double cosa = cos(est_origin.a); - double sina = sin(est_origin.a); - double dx = gpose.x - est_origin.x; - double dy = gpose.y - est_origin.y; - est_pose.x = dx * cosa + dy * sina; - est_pose.y = dy * cosa - dx * sina; + est_pose.a = normalize( gpose.a - est_origin.a ); + double cosa = cos(est_origin.a); + double sina = sin(est_origin.a); + double dx = gpose.x - est_origin.x; + double dy = gpose.y - est_origin.y; + est_pose.x = dx * cosa + dy * sina; + est_pose.y = dy * cosa - dx * sina; - } - break; + } + break; - case STG_POSITION_LOCALIZATION_ODOM: - { - // integrate our velocities to get an 'odometry' position estimate. - double dt = this->world->GetSimInterval()/1e6; + case STG_POSITION_LOCALIZATION_ODOM: + { + // integrate our velocities to get an 'odometry' position estimate. + double dt = this->world->GetSimInterval()/1e6; - est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) ); + est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) ); - double cosa = cos(est_pose.a); - double sina = sin(est_pose.a); - double dx = (vel.x * dt) * (1.0 + integration_error.x ); - double dy = (vel.y * dt) * (1.0 + integration_error.y ); + double cosa = cos(est_pose.a); + double sina = sin(est_pose.a); + double dx = (vel.x * dt) * (1.0 + integration_error.x ); + double dy = (vel.y * dt) * (1.0 + integration_error.y ); - est_pose.x += dx * cosa + dy * sina; - est_pose.y -= dy * cosa - dx * sina; - } - break; + est_pose.x += dx * cosa + dy * sina; + est_pose.y -= dy * cosa - dx * sina; + } + break; - default: - PRINT_ERR2( "unknown localization mode %d for model %s\n", - localization_mode, this->token ); - break; - } + default: + PRINT_ERR2( "unknown localization mode %d for model %s\n", + localization_mode, this->token ); + break; + } - PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n", - est_pose.x, est_pose.y, est_pose.a ); + PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n", + est_pose.x, est_pose.y, est_pose.a ); - Model::Update(); + Model::Update(); } void ModelPosition::Startup( void ) { - Model::Startup(); + Model::Startup(); - PRINT_DEBUG( "position startup" ); + PRINT_DEBUG( "position startup" ); - this->SetWatts( STG_POSITION_WATTS ); + this->SetWatts( STG_POSITION_WATTS ); } void ModelPosition::Shutdown( void ) { - PRINT_DEBUG( "position shutdown" ); + PRINT_DEBUG( "position shutdown" ); - // safety features! - bzero( &goal, sizeof(goal) ); - bzero( &velocity, sizeof(velocity) ); + // safety features! + bzero( &goal, sizeof(goal) ); + bzero( &velocity, sizeof(velocity) ); - this->SetWatts( 0 ); + this->SetWatts( 0 ); - Model::Shutdown(); + Model::Shutdown(); } void ModelPosition::Stop() @@ -530,11 +526,11 @@ assert( ! isnan(vel.z) ); assert( ! isnan(vel.a) ); - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.x = vel.x; - goal.y = vel.y; - goal.z = vel.z; - goal.a = vel.a; + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.x = vel.x; + goal.y = vel.y; + goal.z = vel.z; + goal.a = vel.a; } void ModelPosition::GoTo( double x, double y, double a ) @@ -543,37 +539,37 @@ assert( ! isnan(y) ); assert( ! isnan(a) ); - control_mode = STG_POSITION_CONTROL_POSITION; - goal.x = x; - goal.y = y; - goal.z = 0; - goal.a = a; + control_mode = STG_POSITION_CONTROL_POSITION; + goal.x = x; + goal.y = y; + goal.z = 0; + goal.a = a; } void ModelPosition::GoTo( Pose pose ) { - control_mode = STG_POSITION_CONTROL_POSITION; - goal = pose; + control_mode = STG_POSITION_CONTROL_POSITION; + goal = pose; } /** - Set the current odometry estimate - */ + Set the current odometry estimate +*/ void ModelPosition::SetOdom( Pose odom ) { - est_pose = odom; + est_pose = odom; - // figure out where the implied origin is in global coords - Pose gp = GetGlobalPose(); + // figure out where the implied origin is in global coords + Pose gp = GetGlobalPose(); - double da = -odom.a + gp.a; - double dx = -odom.x * cos(da) + odom.y * sin(da); - double dy = -odom.y * cos(da) - odom.x * sin(da); + double da = -odom.a + gp.a; + double dx = -odom.x * cos(da) + odom.y * sin(da); + double dy = -odom.y * cos(da) - odom.x * sin(da); - // origin of our estimated pose - est_origin.x = gp.x + dx; - est_origin.y = gp.y + dy; - est_origin.a = da; + // origin of our estimated pose + est_origin.x = gp.x + dx; + est_origin.y = gp.y + dy; + est_origin.a = da; } /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ @@ -590,62 +586,61 @@ void ModelPosition::DataVisualize( Camera* cam ) { if( showWaypoints && waypoints && waypoint_count ) - DrawWaypoints(); + DrawWaypoints(); if( showCoords ) - { - // vizualize my estimated pose - glPushMatrix(); + { + // vizualize my estimated pose + glPushMatrix(); - // back into global coords - Gl::pose_inverse_shift( GetGlobalPose() ); + // back into global coords + Gl::pose_inverse_shift( GetGlobalPose() ); - Gl::pose_shift( est_origin ); - PushColor( 1,0,0,1 ); // origin in red - Gl::draw_origin( 0.5 ); + Gl::pose_shift( est_origin ); + PushColor( 1,0,0,1 ); // origin in red + Gl::draw_origin( 0.5 ); - glEnable (GL_LINE_STIPPLE); - glLineStipple (3, 0xAAAA); + glEnable (GL_LINE_STIPPLE); + glLineStipple (3, 0xAAAA); - PushColor( 1,0,0,0.5 ); - glBegin( GL_LINE_STRIP ); - glVertex2f( 0,0 ); - glVertex2f( est_pose.x, 0 ); - glVertex2f( est_pose.x, est_pose.y ); - glEnd(); + PushColor( 1,0,0,0.5 ); + glBegin( GL_LINE_STRIP ); + glVertex2f( 0,0 ); + glVertex2f( est_pose.x, 0 ); + glVertex2f( est_pose.x, est_pose.y ); + glEnd(); - glDisable(GL_LINE_STIPPLE); + glDisable(GL_LINE_STIPPLE); - char label[64]; - snprintf( label, 64, "x:%.3f", est_pose.x ); - Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label ); + char label[64]; + snprintf( label, 64, "x:%.3f", est_pose.x ); + Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label ); - snprintf( label, 64, "y:%.3f", est_pose.y ); - Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const char*)label ); + snprintf( label, 64, "y:%.3f", est_pose.y ); + Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const char*)label ); + + PopColor(); + Gl::pose_shift( est_pose ); + PushColor( 0,1,0,1 ); // pose in green + Gl::draw_origin( 0.5 ); + PopColor(); - PopColor(); - - Gl::pose_shift( est_pose ); - PushColor( 0,1,0,1 ); // pose in green - Gl::draw_origin( 0.5 ); - PopColor(); - - Gl::pose_shift( geom.pose ); - PushColor( 0,0,1,1 ); // offset in blue - Gl::draw_origin( 0.5 ); - PopColor(); + Gl::pose_shift( geom.pose ); + PushColor( 0,0,1,1 ); // offset in blue + Gl::draw_origin( 0.5 ); + PopColor(); - double r,g,b,a; - stg_color_unpack( color, &r, &g, &b, &a ); - PushColor( r, g, b, 0.5 ); + double r,g,b,a; + stg_color_unpack( color, &r, &g, &b, &a ); + PushColor( r, g, b, 0.5 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - blockgroup.DrawFootPrint( geom ); - PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + blockgroup.DrawFootPrint( geom ); + PopColor(); - glPopMatrix(); - } + glPopMatrix(); + } // inherit more viz Model::DataVisualize( cam ); @@ -653,24 +648,31 @@ void ModelPosition::DrawWaypoints() { - glPointSize( 3 ); - - // TRUE + glPointSize( 5 ); glPushMatrix(); PushColor( color ); - + Gl::pose_inverse_shift( pose ); Gl::pose_shift( est_origin ); + // draw waypoints for( unsigned int i=0; i < waypoint_count; i++ ) - waypoints[i].Draw(); + waypoints[i].Draw(); + // draw lines connecting the waypoints + if( waypoint_count > 1 ) + { + glBegin( GL_LINE_STRIP ); + + for( unsigned int i=1; i < waypoint_count; i++ ) + { + glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y ); + glVertex2f( waypoints[i-1].pose.x, waypoints[i-1].pose.y ); + } + + glEnd(); + } + PopColor(); glPopMatrix(); - - /* printf( "pose [%.2f %.2f %.2f %.2f] est_pose [%.2f %.2f %.2f %.2f] est_origin [%.2f %.2f %.2f %.2f]\n", - pose.x, pose.y, pose.z, pose.a, - est_pose.x, est_pose.y, est_pose.z, est_pose.a, - est_origin.x, est_origin.y, est_origin.z, est_origin.a ); - */ } Modified: code/stage/trunk/libstage/waypoint.cc =================================================================== --- code/stage/trunk/libstage/waypoint.cc 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/libstage/waypoint.cc 2009-03-19 06:38:54 UTC (rev 7532) @@ -20,15 +20,15 @@ void Waypoint::Draw() { - GLdouble d[4]; - - d[0] = ((color & 0x00FF0000) >> 16) / 256.0; - d[1] = ((color & 0x0000FF00) >> 8) / 256.0; - d[2] = ((color & 0x000000FF) >> 0) / 256.0; - d[3] = (((color & 0xFF000000) >> 24) / 256.0); - - glColor4dv( d ); - + GLdouble d[4]; + + d[0] = ((color & 0x00FF0000) >> 16) / 256.0; + d[1] = ((color & 0x0000FF00) >> 8) / 256.0; + d[2] = ((color & 0x000000FF) >> 0) / 256.0; + d[3] = (((color & 0xFF000000) >> 24) / 256.0); + + glColor4dv( d ); + glBegin(GL_POINTS); glVertex3f( pose.x, pose.y, pose.z ); glEnd(); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/libstage/world.cc 2009-03-19 06:38:54 UTC (rev 7532) @@ -78,7 +78,6 @@ destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), - powerpack_list( NULL ), ppm( ppm ), // raytrace resolution quit( false ), quit_time( 0 ), @@ -97,12 +96,12 @@ graphics( false ), interval_sim( (stg_usec_t)thousand * interval_sim ), option_table( g_hash_table_new( g_str_hash, g_str_equal ) ), + powerpack_list( NULL ), ray_list( NULL ), sim_time( 0 ), superregions( g_hash_table_new( (GHashFunc)PointIntHash, (GEqualFunc)PointIntEqual ) ), sr_cached(NULL), - // update_list( NULL ), reentrant_update_list( NULL ), nonreentrant_update_list( NULL ), updates( 0 ), @@ -437,6 +436,8 @@ return true; } + dirty = true; // need redraw + // upate all positions first LISTMETHOD( velocity_list, Model*, UpdatePose ); @@ -999,5 +1000,6 @@ void World::StopUpdatingModelPose( Model* mod ) { - velocity_list = g_list_remove( velocity_list, mod ); + // TODO XX figure out how to handle velcoties a bit better + //velocity_list = g_list_remove( velocity_list, mod ); } Modified: code/stage/trunk/stage.pc.in =================================================================== --- code/stage/trunk/stage.pc.in 2009-03-19 02:50:10 UTC (rev 7531) +++ code/stage/trunk/stage.pc.in 2009-03-19 06:38:54 UTC (rev 7532) @@ -1,8 +1,8 @@ prefix=@CMAKE_INSTALL_PREFIX@ Name: stage -Description: Stage robot simulation C library and Player plugin - part of the Player/Stage Project +Description: Stage robot simulation program, C++ library and Player plugin - part of the Player/Stage Project Version: @VERSION@ Requires: glib-2.0 >= 2.4 Libs: -L${prefix}/lib -lstage -Cflags: -I${prefix}/include/Stage-3.0 +Cflags: -I${prefix}/include/Stage-@APIVERSION@ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-20 09:02:42
|
Revision: 7533 http://playerstage.svn.sourceforge.net/playerstage/?rev=7533&view=rev Author: rtv Date: 2009-03-20 09:02:33 +0000 (Fri, 20 Mar 2009) Log Message: ----------- experimenting with wavefront planner Modified Paths: -------------- code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/worlds/simple.world code/stage/trunk/worlds/wavefront.cfg Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-03-20 09:02:33 UTC (rev 7533) @@ -7,18 +7,29 @@ wander ) +# need plaer's wavefront planning library for this one +if( PLAYER_FOUND ) + SET( PLUGINS ${PLUGINS} fasr_plan ) +endif( PLAYER_FOUND ) # create a library module for each plugin and link libstage to each - foreach( PLUGIN ${PLUGINS} ) ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc ) TARGET_LINK_LIBRARIES( ${PLUGIN} stage ) - set_source_files_properties( ${PLUGIN}.cc PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) endforeach( PLUGIN ) # delete the "lib" prefix from the plugin libraries SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" ) +# need plaer's wavefront planning library for this one +if( PLAYER_FOUND ) + link_directories( ${PLAYER_LIBRARY_DIRS} ) + include_directories( ${PLAYER_INCLUDE_DIRS} ) + target_link_libraries( fasr_plan "-lwavefront_standalone" ) +endif( PLAYER_FOUND ) + + # install in <prefix>/lib -INSTALL( TARGETS ${PLUGINS} DESTINATION lib) +install( TARGETS ${PLUGINS} DESTINATION lib) + Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/libstage/block.cc 2009-03-20 09:02:33 UTC (rev 7533) @@ -5,16 +5,16 @@ using namespace Stg; /** Create a new block. A model's body is a list of these - blocks. The point data is copied, so pts can safely be freed - after calling this.*/ + blocks. The point data is copied, so pts can safely be freed + after calling this.*/ Block::Block( Model* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - stg_color_t color, - bool inherit_color - ) : + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + stg_color_t color, + bool inherit_color + ) : mod( mod ), pt_count( pt_count ), pts( (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)) ), @@ -33,15 +33,15 @@ /** A from-file constructor */ Block::Block( Model* mod, - Worldfile* wf, - int entity) + Worldfile* wf, + int entity) : mod( mod ), - pt_count(0), - pts(NULL), - color(0), - inherit_color(true), - rendered_cells( g_ptr_array_sized_new(32) ), - candidate_cells( g_ptr_array_sized_new(32) ) + pt_count(0), + pts(NULL), + color(0), + inherit_color(true), + rendered_cells( g_ptr_array_sized_new(32) ), + candidate_cells( g_ptr_array_sized_new(32) ) { assert(mod); assert(wf); @@ -64,10 +64,10 @@ void Block::Translate( double x, double y ) { for( unsigned int p=0; p<pt_count; p++) - { - pts[p].x += x; - pts[p].y += y; - } + { + pts[p].x += x; + pts[p].y += y; + } // force redraw mod->blockgroup.BuildDisplayList( mod ); @@ -80,10 +80,10 @@ double max = -billion; for( unsigned int p=0; p<pt_count; p++) - { - if( pts[p].y > max ) max = pts[p].y; - if( pts[p].y < min ) min = pts[p].y; - } + { + if( pts[p].y > max ) max = pts[p].y; + if( pts[p].y < min ) min = pts[p].y; + } // return the value half way between max and min return( min + (max - min)/2.0 ); @@ -95,10 +95,10 @@ double max = -billion; for( unsigned int p=0; p<pt_count; p++) - { - if( pts[p].x > max ) max = pts[p].x; - if( pts[p].x < min ) min = pts[p].x; - } + { + if( pts[p].x > max ) max = pts[p].x; + if( pts[p].x < min ) min = pts[p].x; + } // return the value half way between maxx and min return( min + (max - min)/2.0 ); @@ -144,20 +144,20 @@ { // for every cell we are rendered into for( unsigned int i=0; i<rendered_cells->len; i++ ) - { - Cell* cell = (Cell*)g_ptr_array_index( rendered_cells, i); + { + Cell* cell = (Cell*)g_ptr_array_index( rendered_cells, i); - // for every block rendered into that cell - for( GSList* it = cell->list; it; it=it->next ) - { - Block* testblock = (Block*)it->data; - Model* testmod = testblock->mod; + // for every block rendered into that cell + for( GSList* it = cell->list; it; it=it->next ) + { + Block* testblock = (Block*)it->data; + Model* testmod = testblock->mod; - if( !mod->IsRelated( testmod )) - if( ! g_list_find( list, testmod ) ) - list = g_list_append( list, testmod ); - } - } + if( !mod->IsRelated( testmod )) + if( ! g_list_find( list, testmod ) ) + list = g_list_append( list, testmod ); + } + } return list; } @@ -170,29 +170,29 @@ GenerateCandidateCells(); if( mod->vis.obstacle_return ) - // for every cell we may be rendered into - for( unsigned int i=0; i<candidate_cells->len; i++ ) - { - Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i); + // for every cell we may be rendered into + for( unsigned int i=0; i<candidate_cells->len; i++ ) + { + Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i); - // for every rendered into that cell - for( GSList* it = cell->list; it; it=it->next ) - { - Block* testblock = (Block*)it->data; - Model* testmod = testblock->mod; + // for every rendered into that cell + for( GSList* it = cell->list; it; it=it->next ) + { + Block* testblock = (Block*)it->data; + Model* testmod = testblock->mod; - //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); + //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); - // if the tested model is an obstacle and it's not attached to this model - if( (testmod != this->mod) && - testmod->vis.obstacle_return && - !mod->IsRelated( testmod )) - { - //puts( "HIT"); - return testmod; // bail immediately with the bad news - } - } - } + // if the tested model is an obstacle and it's not attached to this model + if( (testmod != this->mod) && + testmod->vis.obstacle_return && + !mod->IsRelated( testmod )) + { + //puts( "HIT"); + return testmod; // bail immediately with the bad news + } + } + } //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this ); return NULL; // no hit @@ -202,13 +202,13 @@ void Block::RemoveFromCellArray( GPtrArray* ptrarray ) { for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); + ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); } void Block::AddToCellArray( GPtrArray* ptrarray ) { for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); + ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); } @@ -276,9 +276,9 @@ // compute the global location of the first point Pose local( (pts[0].x - bgoffset.x) * scale.x , - (pts[0].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); + (pts[0].y - bgoffset.y) * scale.y, + -bgoffset.z, + 0 ); Pose first_gpose, last_gpose; first_gpose = last_gpose = pose_sum( gpose, local ); @@ -289,27 +289,27 @@ // now loop from the the second to the last for( unsigned int p=1; p<pt_count; p++ ) - { - Pose local( (pts[p].x - bgoffset.x) * scale.x , - (pts[p].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); + { + Pose local( (pts[p].x - bgoffset.x) * scale.x , + (pts[p].y - bgoffset.y) * scale.y, + -bgoffset.z, + 0 ); - Pose gpose2 = pose_sum( gpose, local ); + Pose gpose2 = pose_sum( gpose, local ); - // and render the shape of the block into the global cells - mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, - gpose2.x, gpose2.y, - (stg_cell_callback_t)AppendCellToPtrArray, - candidate_cells ); - last_gpose = gpose2; - } + // and render the shape of the block into the global cells + mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, + gpose2.x, gpose2.y, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); + last_gpose = gpose2; + } // close the polygon mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, - first_gpose.x, first_gpose.y, - (stg_cell_callback_t)AppendCellToPtrArray, - candidate_cells ); + first_gpose.x, first_gpose.y, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); mapped = true; } @@ -321,7 +321,7 @@ // extent glBegin( GL_POLYGON); for( unsigned int i=0; i<pt_count; i++ ) - glVertex3f( pts[i].x, pts[i].y, local_z.max ); + glVertex3f( pts[i].x, pts[i].y, local_z.max ); glEnd(); } @@ -330,10 +330,10 @@ // construct a strip that wraps around the polygon glBegin(GL_QUAD_STRIP); for( unsigned int p=0; p<pt_count; p++) - { - glVertex3f( pts[p].x, pts[p].y, local_z.max ); - glVertex3f( pts[p].x, pts[p].y, local_z.min ); - } + { + glVertex3f( pts[p].x, pts[p].y, local_z.max ); + glVertex3f( pts[p].x, pts[p].y, local_z.min ); + } // close the strip glVertex3f( pts[0].x, pts[0].y, local_z.max ); glVertex3f( pts[0].x, pts[0].y, local_z.min ); @@ -344,7 +344,7 @@ { glBegin(GL_POLYGON); for( unsigned int p=0; p<pt_count; p++ ) - glVertex2f( pts[p].x, pts[p].y ); + glVertex2f( pts[p].x, pts[p].y ); glEnd(); } @@ -391,7 +391,7 @@ //printf( "Block::Load entity %d\n", entity ); if( pts ) - stg_points_destroy( pts ); + stg_points_destroy( pts ); pt_count = wf->ReadInt( entity, "points", 0); pts = stg_points_create( pt_count ); @@ -401,10 +401,10 @@ char key[128]; for( unsigned int p=0; p<pt_count; p++ ) { - snprintf(key, sizeof(key), "point[%d]", p ); + snprintf(key, sizeof(key), "point[%d]", p ); - pts[p].x = wf->ReadTupleLength(entity, key, 0, 0); - pts[p].y = wf->ReadTupleLength(entity, key, 1, 0); + pts[p].x = wf->ReadTupleLength(entity, key, 0, 0); + pts[p].y = wf->ReadTupleLength(entity, key, 1, 0); } local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 ); @@ -412,12 +412,12 @@ const char* colorstr = wf->ReadString( entity, "color", NULL ); if( colorstr ) - { - color = stg_lookup_color( colorstr ); - inherit_color = false; - } + { + color = stg_lookup_color( colorstr ); + inherit_color = false; + } else - inherit_color = true; + inherit_color = true; } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/libstage/model.cc 2009-03-20 09:02:33 UTC (rev 7533) @@ -1022,3 +1022,5 @@ //if( world->IsGUI() ) world->RegisterOption( opt ); } + + Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/libstage/model_laser.cc 2009-03-20 09:02:33 UTC (rev 7533) @@ -38,57 +38,57 @@ Option ModelLaser::showLaserBeams( "Laser beams", "show_laser_beams", "", false, NULL ); /** -@ingroup model -@defgroup model_laser Laser model -The laser model simulates a scanning laser rangefinder + @ingroup model + @defgroup model_laser Laser model + The laser model simulates a scanning laser rangefinder -API: Stg::ModelLaser + API: Stg::ModelLaser -<h2>Worldfile properties</h2> + <h2>Worldfile properties</h2> -@par Summary and default values + @par Summary and default values -@verbatim -laser -( - # laser properties - samples 180 - range_max 8.0 - fov 3.14159 - resolution 1 + @verbatim + laser + ( + # laser properties + samples 180 + range_max 8.0 + fov 3.14159 + resolution 1 - # model properties - size [ 0.15 0.15 0.2 ] - color "blue" -) -@endverbatim + # model properties + size [ 0.15 0.15 0.2 ] + color "blue" + ) + @endverbatim -@par Details + @par Details -- samples <int>\n - the number of laser samples per scan -- range_max <float>\n - the maximum range reported by the scanner, in meters. The scanner will not detect objects beyond this range. -- fov <float>\n - the angular field of view of the scanner, in radians. -- resolution <int>\n - Only calculate the true range of every nth laser sample. The missing samples are filled in with a linear interpolation. Generally it would be better to use fewer samples, but some (poorly implemented!) programs expect a fixed number of samples. Setting this number > 1 allows you to reduce the amount of computation required for your fixed-size laser vector. + - samples <int>\n + the number of laser samples per scan + - range_max <float>\n + the maximum range reported by the scanner, in meters. The scanner will not detect objects beyond this range. + - fov <float>\n + the angular field of view of the scanner, in radians. + - resolution <int>\n + Only calculate the true range of every nth laser sample. The missing samples are filled in with a linear interpolation. Generally it would be better to use fewer samples, but some (poorly implemented!) programs expect a fixed number of samples. Setting this number > 1 allows you to reduce the amount of computation required for your fixed-size laser vector. */ - ModelLaser::ModelLaser( World* world, - Model* parent ) +ModelLaser::ModelLaser( World* world, + Model* parent ) : Model( world, parent, MODEL_TYPE_LASER ), - data_dl(0), - data_dirty( true ), - samples( NULL ), // don't allocate sample buffer memory until Update() is called - sample_count( DEFAULT_SAMPLES ), - range_max( DEFAULT_MAXRANGE ), - fov( DEFAULT_FOV ), - resolution( DEFAULT_RESOLUTION ) + data_dl(0), + data_dirty( true ), + samples( NULL ), // don't allocate sample buffer memory until Update() is called + sample_count( DEFAULT_SAMPLES ), + range_max( DEFAULT_MAXRANGE ), + fov( DEFAULT_FOV ), + resolution( DEFAULT_RESOLUTION ) { PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n", - id, typestr ); + id, typestr ); // Model data members @@ -162,8 +162,8 @@ } static bool laser_raytrace_match( Model* hit, - Model* finder, - const void* dummy ) + Model* finder, + const void* dummy ) { // Ignore the model that's looking and things that are invisible to // lasers @@ -186,20 +186,20 @@ rayorg.a = bearing; stg_raytrace_result_t sample = - Raytrace( rayorg, - range_max, - laser_raytrace_match, - NULL, - true ); // z testing enabled + Raytrace( rayorg, + range_max, + laser_raytrace_match, + NULL, + true ); // z testing enabled samples[t].range = sample.range; // if we hit a model and it reflects brightly, we set // reflectance high, else low if( sample.mod && ( sample.mod->vis.laser_return >= LaserBright ) ) - samples[t].reflectance = 1; + samples[t].reflectance = 1; else - samples[t].reflectance = 0; + samples[t].reflectance = 0; // todo - lower bound on range bearing += sample_incr; @@ -209,22 +209,22 @@ if( resolution > 1 ) { for( unsigned int t=resolution; t<sample_count; t+=resolution ) - for( unsigned int g=1; g<resolution; g++ ) - { - if( t >= sample_count ) - break; + for( unsigned int g=1; g<resolution; g++ ) + { + if( t >= sample_count ) + break; - // copy the rightmost sample data into this point - memcpy( &samples[t-g], - &samples[t-resolution], - sizeof(stg_laser_sample_t)); + // copy the rightmost sample data into this point + memcpy( &samples[t-g], + &samples[t-resolution], + sizeof(stg_laser_sample_t)); - double left = samples[t].range; - double right = samples[t-resolution].range; + double left = samples[t].range; + double right = samples[t-resolution].range; - // linear range interpolation between the left and right samples - samples[t-g].range = (left-g*(left-right)/resolution); - } + // linear range interpolation between the left and right samples + samples[t-g].range = (left-g*(left-right)/resolution); + } } data_dirty = true; @@ -317,8 +317,8 @@ { data_dirty = false; - if( data_dl < 1 ) - data_dl = glGenLists(1); + if( data_dl < 1 ) + data_dl = glGenLists(1); glNewList( data_dl, GL_COMPILE ); @@ -330,7 +330,7 @@ glBegin( GL_POINTS ); glVertex2f( 0,0 ); glEnd(); - PopColor(); + PopColor(); // pack the laser hit points into a vertex array for fast rendering static float* pts = NULL; @@ -339,79 +339,79 @@ pts[0] = 0.0; pts[1] = 0.0; - PushColor( 0, 0, 1, 0.5 ); - glDepthMask( GL_FALSE ); - glPointSize( 2 ); + PushColor( 0, 0, 1, 0.5 ); + glDepthMask( GL_FALSE ); + glPointSize( 2 ); - for( unsigned int s=0; s<sample_count; s++ ) - { - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); - pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); + for( unsigned int s=0; s<sample_count; s++ ) + { + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); + pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); - // if the sample is unusually bright, draw a little blob - if( showLaserData && (samples[s].reflectance > 0) ) - { - glBegin( GL_POINTS ); - glVertex2f( pts[2*s+2], pts[2*s+3] ); - glEnd(); - } - } + // if the sample is unusually bright, draw a little blob + if( showLaserData && (samples[s].reflectance > 0) ) + { + glBegin( GL_POINTS ); + glVertex2f( pts[2*s+2], pts[2*s+3] ); + glEnd(); + } + } - glVertexPointer( 2, GL_FLOAT, 0, pts ); + glVertexPointer( 2, GL_FLOAT, 0, pts ); - PopColor(); + PopColor(); - if( showLaserData ) - { - // draw the filled polygon in transparent blue - PushColor( 0, 0, 1, 0.1 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - PopColor(); - } + if( showLaserData ) + { + // draw the filled polygon in transparent blue + PushColor( 0, 0, 1, 0.1 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + PopColor(); + } if( showLaserStrikes ) - { - // draw the beam strike points - PushColor( 0, 0, 1, 0.8 ); - glDrawArrays( GL_POINTS, 0, sample_count+1 ); - PopColor(); - } + { + // draw the beam strike points + PushColor( 0, 0, 1, 0.8 ); + glDrawArrays( GL_POINTS, 0, sample_count+1 ); + PopColor(); + } - if( showLaserFov ) - { - for( unsigned int s=0; s<sample_count; s++ ) - { - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - pts[2*s+2] = (float)(range_max * cos(ray_angle) ); - pts[2*s+3] = (float)(range_max * sin(ray_angle) ); - } + if( showLaserFov ) + { + for( unsigned int s=0; s<sample_count; s++ ) + { + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + pts[2*s+2] = (float)(range_max * cos(ray_angle) ); + pts[2*s+3] = (float)(range_max * sin(ray_angle) ); + } - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - PushColor( 0, 0, 1, 0.5 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - PopColor(); - // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + PushColor( 0, 0, 1, 0.5 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + PopColor(); + // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - } + } - if( showLaserBeams ) - { - PushColor( 0, 0, 1, 0.5 ); - glBegin( GL_LINES ); + if( showLaserBeams ) + { + PushColor( 0, 0, 1, 0.5 ); + glBegin( GL_LINES ); - for( unsigned int s=0; s<sample_count; s++ ) - { + for( unsigned int s=0; s<sample_count; s++ ) + { - glVertex2f( 0,0 ); - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - glVertex2f( samples[s].range * cos(ray_angle), - samples[s].range * sin(ray_angle) ); + glVertex2f( 0,0 ); + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + glVertex2f( samples[s].range * cos(ray_angle), + samples[s].range * sin(ray_angle) ); - } - glEnd(); - PopColor(); - } + } + glEnd(); + PopColor(); + } glDepthMask( GL_TRUE ); Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/libstage/model_position.cc 2009-03-20 09:02:33 UTC (rev 7533) @@ -662,7 +662,7 @@ // draw lines connecting the waypoints if( waypoint_count > 1 ) { - glBegin( GL_LINE_STRIP ); + glBegin( GL_LINES ); for( unsigned int i=1; i < waypoint_count; i++ ) { Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/libstage/stage.cc 2009-03-20 09:02:33 UTC (rev 7533) @@ -254,7 +254,7 @@ } } -// // returns TRUE if any channel in the pixel is non-zero +// returns true if the value in the first channel is above threshold static gboolean pb_pixel_is_set( Fl_Shared_Image* img, int x, int y, int threshold ) { guchar* pixel = pb_get_pixel( img,x,y ); Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/worlds/simple.world 2009-03-20 09:02:33 UTC (rev 7533) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 50 # real-time interval between simulation updates in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds paused 0 @@ -44,5 +44,9 @@ sicklaser() ctrl "wander" + + # report error-free position in world coordinates + localization "gps" + localization_origin [ 0 0 0 0 ] ) Modified: code/stage/trunk/worlds/wavefront.cfg =================================================================== --- code/stage/trunk/worlds/wavefront.cfg 2009-03-19 06:38:54 UTC (rev 7532) +++ code/stage/trunk/worlds/wavefront.cfg 2009-03-20 09:02:33 UTC (rev 7533) @@ -1,23 +1,34 @@ driver ( name "stage" - plugin "libstageplugin" + plugin "stageplugin" provides ["6665:simulation:0"] - worldfile "simple.world" + worldfile "wavefront.world" ) +#driver +#( +# name "stage" +# provides ["6665:map:0"] +# model "cave" +#) + driver ( - name "stage" + name "mapfile" provides ["6665:map:0"] - model "cave" + filename "bitmaps/cave.png" + resolution 0.032 # meters per pixel + negate 0 + origin [-8 -8 ] # real-world location of the bottom-left-hand corner of the map ) + driver ( name "stage" provides ["6665:position2d:0" "6665:laser:0"] - model "robot1" + model "r0" ) driver @@ -36,19 +47,19 @@ # requires ["output::6665:position2d:0" "input::6665:position2d:0" "6665:laser:0"] #) -driver -( - name "amcl" - provides ["6665:localize:0" "6665:position2d:2"] - requires ["odometry::6665:position2d:0" "6665:laser:0" "laser::6665:map:0"] -) +#driver +#( +# name "amcl" +# provides ["6665:localize:0" "6665:position2d:2"] +# requires ["odometry::6665:position2d:0" "6665:laser:0" "laser::6665:map:0"] +#) driver ( name "wavefront" provides ["6665:planner:0"] - requires ["output::6665:position2d:1" "input::6665:position2d:2" "6665:map:0"] + requires ["output::6665:position2d:1" "input::6665:position2d:0" "6665:map:0"] safety_dist 0.15 distance_epsilon 0.5 angle_epsilon 10 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-21 01:06:03
|
Revision: 7536 http://playerstage.svn.sourceforge.net/playerstage/?rev=7536&view=rev Author: rtv Date: 2009-03-21 01:05:53 +0000 (Sat, 21 Mar 2009) Log Message: ----------- exploring planner Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr_plan.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/fasr_plan.world Modified: code/stage/trunk/examples/ctrl/fasr_plan.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr_plan.cc 2009-03-20 18:56:50 UTC (rev 7535) +++ code/stage/trunk/examples/ctrl/fasr_plan.cc 2009-03-21 01:05:53 UTC (rev 7536) @@ -16,35 +16,12 @@ const double cruisespeed = 0.4; const double avoidspeed = 0.05; const double avoidturn = 0.5; -const double minfrontdistance = 0.7; -const double stopdist = 0.5; +const double minfrontdistance = 0.4; +const double stopdist = 0.3; const int avoidduration = 10; const int workduration = 20; const int payload = 1; -double have[4][4] = { - // { -120, -180, 180, 180 }, - //{ -90, -120, 180, 90 }, - { 90, 180, 180, 180 }, - { 90, -90, 180, 90 }, - { 90, 90, 180, 90 }, - { 0, 45, 0, 0} -}; - -double need[4][4] = { - { -120, -180, 180, 180 }, - { -90, -120, 180, 90 }, - { -90, -90, 180, 180 }, - { -90, -180, -90, -90 } -}; - -double refuel[4][4] = { - { 0, 0, 45, 120 }, - { 0,-90, -60, -160 }, - { -90, -90, 180, 180 }, - { -90, -180, -90, -90 } -}; - typedef enum { MODE_WORK=0, MODE_DOCK, @@ -169,33 +146,64 @@ img->release(); // frees all image resources plan_init(plan); - plan_compute_cspace(plan); - //print_cspace(plan); -// double x, y; -// plan_convert_waypoint( plan, plan->cells, &x, &y ); -// printf( "cell 0 (%.2f, %.2f)\n", x, y ); + return plan; +} -// plan_convert_waypoint( plan, plan->cells + sy*sx -1, &x, &y ); -// printf( "cell end (%.2f, %.2f)\n", x, y ); -// plan_convert_waypoint( plan, plan->cells + sx, &x, &y ); -// printf( "cell sx (%.2f, %.2f)\n", x, y ); +class PointsVis : public CustomVisualizer +{ +public: + PointsVis( Model* mod ) : + mod( mod ), + pts( NULL ), + pt_count( 0 ), + CustomVisualizer() + { /* nothing to do */ }; + + virtual void DataVisualize( Camera* cam ) + { + if( pt_count < 1 ) + return; + + glPushMatrix(); - //plan_convert_waypoint( plan, plan->cells, &x, &y ); - //printf( "cell 0 (%.2f, %.2f)\n", x, y ); + Gl::pose_inverse_shift( mod->GetGlobalPose() ); + + glColor3f( 0,1,0 ); + glBegin( GL_POINTS ); + + for( int i=0; i< pt_count; i++ ) + glVertex2f( pts[2*i], pts[2*i+1] ); + + glEnd(); + + glPopMatrix(); + } - // getchar(); + // rtv - surely a static string member would be easier here? + //must return a name for visualization (careful not to return stack-memory) + + static const std::string n; + virtual const std::string& name() { return n; } ; + + void SetPoints( double* pts, int pt_count ) + { + this->pts = pts; + this->pt_count = pt_count; + } + +private: + Model* mod; + double* pts; + int pt_count; +}; - puts( "PLAN CREATED" ); +const std::string PointsVis::n = "PointsVisName"; - return plan; -} - - class Robot { private: @@ -218,6 +226,14 @@ plan_t* plan; bool plan_done; + Model* goal; + + GQueue* laser_history; + double* pts; + + PointsVis* vis; + bool havepath; + public: Robot( ModelPosition* pos, Model* source, @@ -241,7 +257,12 @@ mode(MODE_WORK), at_dest( false ), plan( NULL ), - plan_done( false ) + plan_done( false ), + goal( NULL ), + laser_history( g_queue_new() ), + pts( NULL ), + vis( new PointsVis( laser ) ), + havepath( false ) { // need at least these models to get any work done // (pos must be good, as we used it in the initialization list) @@ -272,11 +293,13 @@ //planif( !plan ) plan = SetupPlan( 0.3, // robot radius - 0.05, // safety dist - 0.5, // max radius + 0.1, // safety dist + 0.6, // max radius 1.0, // dist penalty "/Users/vaughan/PS/stage/trunk/worlds/bitmaps/cave_compact.png", 16.0, -8.0, -8.0, 250 ); + + laser->AddCustomVisualizer( vis ); } @@ -448,62 +471,61 @@ void Work() { - if( ! ObstacleAvoid() ) - { - if( verbose ) puts( "Cruise" ); - - ModelGripper::config_t gdata = gripper->GetConfig(); - - //avoidcount = 0; - pos->SetXSpeed( cruisespeed ); - - Pose pose = pos->GetPose(); - - int x = (pose.x + 8) / 4; - int y = (pose.y + 8) / 4; - - // oh what an awful bug - 5 hours to track this down. When using - // this controller in a world larger than 8*8 meters, a_goal can - // sometimes be NAN. Causing trouble WAY upstream. - if( x > 3 ) x = 3; - if( y > 3 ) y = 3; - if( x < 0 ) x = 0; - if( y < 0 ) y = 0; - - double a_goal = - dtor( ( pos->GetFlagCount() || gdata.gripped ) ? have[y][x] : need[y][x] ); - - // if we are low on juice - find the direction to the recharger instead - if( Hungry() ) - { - //puts( "hungry - using refuel map" ); - - // use the refuel map - a_goal = dtor( refuel[y][x] ); + Pose pose = pos->GetPose(); + plan_update_waypoints( plan, pose.x, pose.y ); - if( charger_ahoy ) // I see a charger while hungry! - mode = MODE_DOCK; - } - else - { - if( ! at_dest ) - { - if( gdata.beam[0] ) // inner break beam broken - gripper->CommandClose(); - } - } + if( ObstacleAvoid() ) + { + //pos->SetSpeed( 0,0,0 ); - assert( ! isnan(a_goal ) ); - assert( ! isnan(pose.a ) ); + Pose gpose = goal->GetGlobalPose(); + + if( plan_do_global( plan, pose.x, pose.y, gpose.x, gpose.y ) < 0) + { + havepath = false; + } + } + else + { + if( verbose ) puts( "Cruise" ); - double a_error = normalize( a_goal - pose.a ); - - assert( ! isnan(a_error) ); - - pos->SetTurnSpeed( a_error ); + if( havepath ) + pos->SetXSpeed( cruisespeed ); + else + pos->SetXSpeed( 0 ); + + Pose pose = pos->GetPose(); + + double gx,gy; + plan_convert_waypoint( plan, plan->waypoints[1], &gx, &gy ); + + double a_goal = atan2( gy-pose.y, gx-pose.x ); + + // printf( "goal heading %.2f\n", a_goal ); + + // if we are low on juice - find the direction to the recharger instead + if( Hungry() ) + { + //puts( "hungry - using refuel map" ); + + // use the refuel map + //a_goal = dtor( refuel[y][x] ); + + //if( charger_ahoy ) // I see a charger while hungry! + //mode = MODE_DOCK; + } + + double a_error = normalize( a_goal - pose.a ); + pos->SetTurnSpeed( a_error ); } } + + typedef struct + { + stg_laser_sample_t* scan; + Pose pose; + } scan_record_t; // inspect the laser data and decide what to do static int LaserUpdate( ModelLaser* laser, Robot* robot ) @@ -514,73 +536,113 @@ uint32_t sample_count=0; stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); - + if( scan == NULL ) return 0; - Pose gpose = laser->GetGlobalPose(); - + scan_record_t* sr = new scan_record_t; + sr->pose = laser->GetGlobalPose(); + + // deep copy the samples + sr->scan = new stg_laser_sample_t[sample_count]; + memcpy( sr->scan, scan, sample_count * sizeof(stg_laser_sample_t)); + + // add the copy to the list + g_queue_push_tail( robot->laser_history, sr ); + + // take off the oldest scan and free it + if( robot->laser_history->length > 10 ) + { + scan_record_t* old = (scan_record_t*)g_queue_pop_head( robot->laser_history ); + assert( old ); + delete[] old->scan; + delete old; + } + // fill the dynamic map - double pts[2*sample_count]; + robot->pts = (double*)realloc( robot->pts, sizeof(double) * 2*sample_count*robot->laser_history->length ); // project hit points from each scan // scan = this->scans; int scan_points_count = 0; - //for(int i=0;i<this->scans_count;i++,scan++) - // { - - stg_laser_cfg_t cfg = laser->GetConfig(); - float b = -cfg.fov / 2.0; - - for( unsigned int j=0; j<sample_count; j++ ) - { - if( scan[j].range >= cfg.range_bounds.max ) - continue; - - double cs,sn; - cs = cos(gpose.a+b); - sn = sin(gpose.a+b); - - double lx,ly; - lx = gpose.x + scan[j].range*cs; - ly = gpose.y + scan[j].range*sn; - - //assert(this->scan_points_count*2 < this->scan_points_size); - pts[2*j ] = lx; - pts[2*j+1] = ly; - scan_points_count++; - } - - //printf("setting %d hit points\n", this->scan_points_count); - plan_set_obstacles( robot->plan, pts, scan_points_count); + for( int i = 0; i < robot->laser_history->length; i++ ) + { + scan_record_t* scr = (scan_record_t*)g_queue_peek_nth( robot->laser_history, i ); + stg_laser_sample_t* ascan = scr->scan; + Pose apose = scr->pose; + + double amin = -cfg.fov / 2.0; + double aincr = cfg.fov / sample_count; + + for( unsigned int j=0; j<sample_count; j++ ) + { + if( ascan[j].range >= cfg.range_bounds.max ) + continue; + + double a = amin + j * aincr; + double cs,sn; + cs = cos(apose.a+a); + sn = sin(apose.a+a); + + double lx,ly; + lx = apose.x + ascan[j].range*cs; + ly = apose.y + ascan[j].range*sn; + + //assert(this->scan_points_count*2 < this->scan_points_size); + robot->pts[2*scan_points_count ] = lx; + robot->pts[2*scan_points_count+1] = ly; + scan_points_count++; + } + } + //printf("setting %d hit points\n", scan_points_count); + plan_set_obstacles( robot->plan, robot->pts, scan_points_count); + + robot->vis->SetPoints( robot->pts, scan_points_count ); + + // Waypoint* wps = new Waypoint[scan_points_count]; +// for( int j=0; j<scan_points_count; j++ ) +// { +// wps[j].pose.x = pts[2*j]; +// wps[j].pose.y = pts[2*j+1]; +// wps[j].pose.z = 0; +// wps[j].pose.a = 0; +// wps[j].color = stg_color_pack( 0,1,0,0 ); +// } + +// Waypoint* oldp = ((ModelPosition*)robot->pos->GetWorld()->GetModel( "dummy" ))->SetWaypoints( wps, scan_points_count ); +// if( oldp ) +// delete[] oldp; + +// delete[] pts; + switch( robot->mode ) { case MODE_DOCK: - //puts( "DOCK" ); - robot->Dock(); - break; - + //puts( "DOCK" ); + robot->Dock(); + break; + case MODE_WORK: - //puts( "WORK" ); - robot->Work(); - break; - + //puts( "WORK" ); + robot->Work(); + break; + case MODE_UNDOCK: - //puts( "UNDOCK" ); - robot->UnDock(); - break; - + //puts( "UNDOCK" ); + robot->UnDock(); + break; + default: - printf( "unrecognized mode %u\n", robot->mode ); + printf( "unrecognized mode %u\n", robot->mode ); } - + return 0; } @@ -606,72 +668,122 @@ //printf( "planning from (%.2f %.2f) to (%.2f %.2f)\n", // pose.x, pose.y, -4.0, 0.0 ); - - if( 1 )//! robot->plan_done ) - { - if( plan_do_global( plan, pose.x, pose.y, -7,-7 ) < 0) - { - puts( "no global path" ); - } - else - robot->plan_done = true; - } - + Model* nextgoal = NULL; + + if( pos->GetFlagCount() ) + nextgoal = robot->sink; + else + nextgoal = robot->source; + + if( (robot->goal != nextgoal) )//|| !robot->plan_done) + { + robot->goal = nextgoal; + + Pose gpose = robot->goal->GetGlobalPose(); + if( plan_do_global( plan, pose.x, pose.y, gpose.x, gpose.y ) < 0) + { + puts( "no global path" ); + } + } + //printf( "PATH_COUNT %d\n", plan->path_count ); + + +// double vx, va; +// int rotatedir; + +// plan_compute_diffdrive_cmds( plan, +// &vx, &va, +// &rotatedir, +// pose.x, pose.y, pose.a, // lx, ly, la +// -7, -7, 0, // gx, gy, ga +// 0.1, 0.1, // goal_d, goal_a +// 1.0, 5.0, // maxd, dweight +// 0.0, 0.2, // tvmin, tvmax +// 0.0, 2, // avmin, avmax +// 0, 1 ); // amin, amax +// // double dx, da; +// // plan_get_carrot( plan, &dx, &da, +// // pose.x, pose.y, +// // 2.0, 10 ); +// pos->SetSpeed( vx, 0, va ); + + +// if( plan_do_local( plan, pose.x, pose.y, 1.0 ) < 0 ) +// { +// puts( "no local path" ); + +// Pose gpose = robot->goal->GetGlobalPose(); + +// if( plan_do_global( plan, pose.x, pose.y, gpose.x, gpose.y ) < 0) +// { +// robot->havepath = false; +// puts( "no global path" ); +// } +// else +// robot->havepath = true; +// } +// else + robot->havepath = true; + plan_update_waypoints( plan, pose.x, pose.y ); - + //printf( "WAYPOINT_COUNT %d\n", plan->waypoint_count ); - - + Waypoint* wps = NULL; stg_color_t col = pos->GetColor(); - + if( plan->waypoint_count > 0 ) { - wps = new Waypoint[plan->waypoint_count]; - for( int i=0; i<plan->waypoint_count; i++ ) - { - //plan_convert_waypoint( plan, plan->path[i], - // &wps[i].pose.x, - // &wps[i].pose.y ); - - plan_convert_waypoint( plan, plan->waypoints[i], - &wps[i].pose.x, - &wps[i].pose.y ); - - wps[i].color = col; - } + wps = new Waypoint[plan->waypoint_count]; + for( int i=0; i<plan->waypoint_count; i++ ) + { + //plan_convert_waypoint( plan, plan->path[i], + // &wps[i].pose.x, + // &wps[i].pose.y ); + + plan_convert_waypoint( plan, plan->waypoints[i], + &wps[i].pose.x, + &wps[i].pose.y ); + + wps[i].color = col; + } } - - Waypoint* old = pos->SetWaypoints( wps, plan->waypoint_count ); - - if( old ) - delete[] old; - + + + Waypoint* old = pos->SetWaypoints( wps, plan->waypoint_count ); + if( old ) + delete[] old; + + //pose.z += 0.0001; //robot->pos->SetPose( pose ); + Pose psrc = robot->source->GetPose(); + if( pos->GetFlagCount() < payload && - hypot( -7-pose.x, -7-pose.y ) < 2.0 ) + hypot(psrc.x-pose.x, psrc.y-pose.y ) < 2.0 ) { - if( ++robot->work_get > workduration ) - { - // protect source from concurrent access - robot->source->Lock(); - - // transfer a chunk from source to robot - pos->PushFlag( robot->source->PopFlag() ); - robot->source->Unlock(); - - robot->work_get = 0; - } + if( ++robot->work_get > workduration ) + { + // protect source from concurrent access + robot->source->Lock(); + + // transfer a chunk from source to robot + pos->PushFlag( robot->source->PopFlag() ); + robot->source->Unlock(); + + robot->work_get = 0; + } } robot->at_dest = false; - if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) + Pose psink = robot->sink->GetPose(); + + if( hypot( psink.x-pose.x, psink.y-pose.y ) < 1.0 ) { robot->at_dest = true; @@ -773,3 +885,4 @@ + Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-03-20 18:56:50 UTC (rev 7535) +++ code/stage/trunk/libstage/model_load.cc 2009-03-21 01:05:53 UTC (rev 7536) @@ -246,12 +246,16 @@ this->pose.y, this->pose.a ); + // just in case + pose.a = normalize( pose.a ); + geom.pose.a = normalize( geom.pose.a ); + if( wf->PropertyExists( wf_entity, "pose" ) ) { wf->WriteTupleLength( wf_entity, "pose", 0, this->pose.x); wf->WriteTupleLength( wf_entity, "pose", 1, this->pose.y); wf->WriteTupleLength( wf_entity, "pose", 2, this->pose.z); - wf->WriteTupleAngle( wf_entity, "pose", 3, this->pose.a); + wf->WriteTupleAngle( wf_entity, "pose", 3, this->pose.a ); } if( wf->PropertyExists( wf_entity, "size" ) ) Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-20 18:56:50 UTC (rev 7535) +++ code/stage/trunk/libstage/model_position.cc 2009-03-21 01:05:53 UTC (rev 7536) @@ -87,7 +87,7 @@ const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT = STG_POSITION_DRIVE_DIFFERENTIAL; Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "", false, NULL ); -Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", false, NULL ); +Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", true, NULL ); ModelPosition::ModelPosition( World* world, Model* parent ) @@ -151,23 +151,23 @@ if( wf->PropertyExists( wf_entity, "drive" ) ) { const char* mode_str = - wf->ReadString( wf_entity, "drive", NULL ); - - if( mode_str ) - { - if( strcmp( mode_str, "diff" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL; - else if( strcmp( mode_str, "omni" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_OMNI; - else if( strcmp( mode_str, "car" ) == 0 ) - drive_mode = STG_POSITION_DRIVE_CAR; - else - { - PRINT_ERR1( "invalid position drive mode specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.", mode_str ); - } - } + wf->ReadString( wf_entity, "drive", NULL ); + + if( mode_str ) + { + if( strcmp( mode_str, "diff" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL; + else if( strcmp( mode_str, "omni" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_OMNI; + else if( strcmp( mode_str, "car" ) == 0 ) + drive_mode = STG_POSITION_DRIVE_CAR; + else + { + PRINT_ERR1( "invalid position drive mode specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.", mode_str ); + } + } } - + // load odometry if specified if( wf->PropertyExists( wf_entity, "odom" ) ) { @@ -183,12 +183,12 @@ // specified est_origin = this->GetGlobalPose(); - if( wf->PropertyExists( wf_entity, "localization_origin" ) ) - { - est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0, est_origin.x ); - est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1, est_origin.y ); - est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2, est_origin.z ); - est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3, est_origin.a ); + if( wf->PropertyExists( wf_entity, "localization_origin" ) ) + { + est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0, est_origin.x ); + est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1, est_origin.y ); + est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2, est_origin.z ); + est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3, est_origin.a ); // compute our localization pose based on the origin and true pose Pose gpose = this->GetGlobalPose(); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-20 18:56:50 UTC (rev 7535) +++ code/stage/trunk/worlds/fasr.world 2009-03-21 01:05:53 UTC (rev 7536) @@ -14,7 +14,7 @@ # threads may speed things up here depending on available CPU cores & workload # threadpool 8 - threadpool 16 + threadpool 0 # configure the GUI window Modified: code/stage/trunk/worlds/fasr_plan.world =================================================================== --- code/stage/trunk/worlds/fasr_plan.world 2009-03-20 18:56:50 UTC (rev 7535) +++ code/stage/trunk/worlds/fasr_plan.world 2009-03-21 01:05:53 UTC (rev 7536) @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 180 range_max 5 laser_return 2 watts 30 fov 360) + sicklaser( samples 64 range_max 5 laser_return 2 watts 30 fov 180 size [0.3 0.3 0.2 ]) ctrl "fasr_plan" joules 100000 joules_capacity 400000 @@ -89,6 +89,8 @@ localization "gps" localization_origin [ 0 0 0 0 ] + + show_waypoints 1 ) define charge_station model @@ -124,6 +126,8 @@ obstacle_return 0 ) +position( pose [ 0 0 0 0 ] size [ 1 1 1 ] name "dummy" ) + #puck( pose [ 1.175 2.283 0 0 ] ) #puck( pose [ 0.875 3.139 0 0 ] ) #puck( pose [ 1.043 2.825 0 0 ] ) @@ -160,9 +164,9 @@ autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) -autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) -autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) -autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) +#autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) +#autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) +#autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) #autorob( pose [7.394 5.595 0 129.497] joules 400000 ) #autorob( pose [6.468 6.708 0 170.743] joules 100000 ) #autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-25 17:56:08
|
Revision: 7545 http://playerstage.svn.sourceforge.net/playerstage/?rev=7545&view=rev Author: rtv Date: 2009-03-25 17:56:06 +0000 (Wed, 25 Mar 2009) Log Message: ----------- made update callbacks thread safe, added Model::GetId() and disabled wavefront-based fasr_plan demo Modified Paths: -------------- code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-03-25 17:56:06 UTC (rev 7545) @@ -8,9 +8,9 @@ ) # need plaer's wavefront planning library for this one -if( PLAYER_FOUND ) - SET( PLUGINS ${PLUGINS} fasr_plan ) -endif( PLAYER_FOUND ) +#if( PLAYER_FOUND ) +# SET( PLUGINS ${PLUGINS} fasr_plan ) +#endif( PLAYER_FOUND ) # create a library module for each plugin and link libstage to each Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/libstage/model.cc 2009-03-25 17:56:06 UTC (rev 7545) @@ -665,7 +665,10 @@ void Model::UpdateIfDue( void ) { if( UpdateDue() ) - Update(); + { + Update(); + CallUpdateCallbacks(); + } } bool Model::UpdateDue( void ) @@ -699,11 +702,16 @@ */ } + //CallCallbacks( &hooks.update ); + //last_update = world->sim_time; +} + +void Model::CallUpdateCallbacks( void ) +{ CallCallbacks( &hooks.update ); last_update = world->sim_time; } - stg_meters_t Model::ModelHeight() { stg_meters_t m_child = 0; //max size of any child Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/libstage/model_getset.cc 2009-03-25 17:56:06 UTC (rev 7545) @@ -1,6 +1,7 @@ #include "stage.hh" using namespace Stg; + void Model::SetGeom( Geom geom ) { UnMapWithChildren(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/libstage/stage.hh 2009-03-25 17:56:06 UTC (rev 7545) @@ -1720,7 +1720,8 @@ bool UpdateDue( void ); void UpdateIfDue(); - + void CallUpdateCallbacks( void ); + void DrawBlocksTree(); virtual void DrawBlocks(); void DrawBoundingBox(); @@ -1938,7 +1939,10 @@ void SetFiducialKey( int key ); stg_color_t GetColor(){ return color; } - + + /** return a model's unique process-wide identifier */ + uint32_t GetId() { return id; } + // stg_laser_return_t GetLaserReturn(){ return laser_return; } /** Change a model's parent - experimental*/ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/libstage/world.cc 2009-03-25 17:56:06 UTC (rev 7545) @@ -475,6 +475,9 @@ while( update_jobs_pending ) g_cond_wait( worker_threads_done, thread_mutex ); g_mutex_unlock( thread_mutex ); + + // now call all the callbacks - ignores dueness, but not a big deal + LISTMETHOD( reentrant_update_list, Model*, CallUpdateCallbacks ); } this->sim_time += this->interval_sim; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-25 17:31:27 UTC (rev 7544) +++ code/stage/trunk/worlds/fasr.world 2009-03-25 17:56:06 UTC (rev 7545) @@ -13,8 +13,8 @@ resolution 0.02 # threads may speed things up here depending on available CPU cores & workload -# threadpool 8 - threadpool 0 + threadpool 8 +# threadpool 0 # configure the GUI window This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-27 02:13:42
|
Revision: 7547 http://playerstage.svn.sourceforge.net/playerstage/?rev=7547&view=rev Author: rtv Date: 2009-03-27 02:13:08 +0000 (Fri, 27 Mar 2009) Log Message: ----------- added worlfile support for loading property database Modified Paths: -------------- code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-03-25 22:13:27 UTC (rev 7546) +++ code/stage/trunk/libstage/model_load.cc 2009-03-27 02:13:08 UTC (rev 7547) @@ -198,18 +198,26 @@ if( wf->PropertyExists( wf_entity, "map_resolution" )) this->SetMapResolution( wf->ReadFloat(wf_entity, "map_resolution", this->map_resolution )); + + + // populate the key-value database + if( wf->PropertyExists( wf_entity, "db_count" ) ) + LoadDataBaseEntries( wf, wf_entity ); if( wf->PropertyExists( wf_entity, "ctrl" )) { char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL ); + + //char* argstr = (char*)wf->ReadString(wf_entity, "ctrl_argstr", NULL ); + if( !lib ) - printf( "Error - NULL library name specified for model %s\n", token ); + printf( "Error - NULL library name specified for model %s\n", token ); else - LoadControllerModule( lib ); + LoadControllerModule( lib ); } - + if( wf->PropertyExists( wf_entity, "say" )) this->Say( wf->ReadString(wf_entity, "say", NULL )); @@ -300,29 +308,77 @@ this->initfunc = (ctrlinit_t*)lt_dlsym( handle, "Init" ); if( this->initfunc == NULL ) - { - printf( "Libtool error: %s. Something is wrong with your plugin. Quitting\n", - lt_dlerror() ); // report the error from libtool - puts( "libtool error #1" ); - fflush( stdout ); - exit(-1); - } + { + printf( "Libtool error: %s. Something is wrong with your plugin. Quitting\n", + lt_dlerror() ); // report the error from libtool + puts( "libtool error #1" ); + fflush( stdout ); + exit(-1); + } } else { printf( "Libtool error: %s. Can't open your plugin controller. Quitting\n", - lt_dlerror() ); // report the error from libtool - + lt_dlerror() ); // report the error from libtool + PRINT_ERR1( "Failed to open \"%s\". Check that it can be found by searching the directories in your STAGEPATH environment variable, or the current directory if STAGEPATH is not set.]\n", lib ); puts( "libtool error #2" ); fflush( stdout ); exit(-1); } - + fflush(stdout); // as we now have a controller, the world needs to call our update function StartUpdating(); } + + void Model::LoadDataBaseEntries( Worldfile* wf, int entity ) + { + int entry_count = wf->ReadInt( entity, "db_count", 0 ); + if( entry_count < 1 ) + return; + + for( int e=0; e<entry_count; e++ ) + { + const char* entry = wf->ReadTupleString( entity, "db", e, NULL ); + + const size_t SZ = 64; + char key[SZ], type[SZ], value[SZ]; + + sscanf( entry, "%[^<]<%[^>]>%[^\"]", key, type, value ); + + assert( key ); + assert( type ); + assert( value ); + + printf( "\nkey: %s type: %s value: %s\n", + key, type, value ); + + if( strcmp( type, "int" ) == 0 ) + { + int* i = new int( atoi(value) ); + SetProperty( strdup(key), (void*)i ); + } + else if( strcmp( type, "float" ) == 0 ) + { + float* f = new float( strtod(value, NULL) ); + SetProperty( strdup(key), (void*)f ); + } + else if( strcmp( type, "string" ) == 0 ) + SetProperty( strdup(key), (void*)strdup(value) ); + else + PRINT_ERR1( "unknown database entry type \"%s\"\n", type ); + +// int* i = (int*)GetProperty( key ); +// float* f = (float*)GetProperty( key ); +// char* c = (char*)GetProperty( key ); + +// printf( "property %s has int value %d\n", key, *i ); +// printf( "property %s has float value %.2f\n", key, *f ); +// printf( "property %s has char* value %s\n", key, c ); + } + + } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-25 22:13:27 UTC (rev 7546) +++ code/stage/trunk/libstage/stage.hh 2009-03-27 02:13:08 UTC (rev 7547) @@ -1770,6 +1770,8 @@ void DrawFlagList(); void DrawPose( Pose pose ); + + void LoadDataBaseEntries( Worldfile* wf, int entity ); public: PowerPack* FindPowerPack(); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-25 22:13:27 UTC (rev 7546) +++ code/stage/trunk/worlds/fasr.world 2009-03-27 02:13:08 UTC (rev 7547) @@ -169,7 +169,7 @@ autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) -autorob( pose [6.362 5.632 0 163.239] joules 400000 ) +autorob( pose [6.362 5.632 0 163.239] joules 400000 db_count 3 db [ "foo<int>42" "bar<float>6.75" "bash<string>my old shoe" ] ) #autorob( pose [7.559 4.764 0 -139.066] ) #autorob( pose [5.471 7.446 0 77.301] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-28 21:27:19
|
Revision: 7555 http://playerstage.svn.sourceforge.net/playerstage/?rev=7555&view=rev Author: rtv Date: 2009-03-28 21:26:54 +0000 (Sat, 28 Mar 2009) Log Message: ----------- cleaning up, using references for common method args and improving const-correctness Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/examples/ctrl/sink.cc code/stage/trunk/examples/ctrl/source.cc code/stage/trunk/examples/ctrl/wander.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -390,23 +390,18 @@ //pose.z += 0.0001; //robot->pos->SetPose( pose ); - + if( pos->GetFlagCount() < payload && hypot( -7-pose.x, -7-pose.y ) < 2.0 ) { if( ++robot->work_get > workduration ) { - // protect source from concurrent access - robot->source->Lock(); - // transfer a chunk from source to robot pos->PushFlag( robot->source->PopFlag() ); - robot->source->Unlock(); - robot->work_get = 0; - } + } } - + robot->at_dest = false; if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) @@ -417,16 +412,10 @@ if( ++robot->work_put > workduration ) { - // protect sink from concurrent access - robot->sink->Lock(); - //puts( "dropping" ); // transfer a chunk between robot and goal robot->sink->PushFlag( pos->PopFlag() ); - robot->sink->Unlock(); - robot->work_put = 0; - } } Modified: code/stage/trunk/examples/ctrl/sink.cc =================================================================== --- code/stage/trunk/examples/ctrl/sink.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/examples/ctrl/sink.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -7,11 +7,10 @@ // Stage calls this when the model starts up extern "C" int Init( Model* mod ) -{ +{ + //for( int i=0; i<5; i++ ) + // mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), 0.5 ) ); - for( int i=0; i<5; i++ ) - mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), 0.5 ) ); - mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); return 0; //ok @@ -20,14 +19,8 @@ // inspect the laser data and decide what to do int Update( Model* mod, void* dummy ) { - // protect access to this model from other controllers - mod->Lock(); - if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 ) mod->PopFlag(); - - mod->Unlock(); - return 0; // run again } Modified: code/stage/trunk/examples/ctrl/source.cc =================================================================== --- code/stage/trunk/examples/ctrl/source.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/examples/ctrl/source.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -21,14 +21,9 @@ // inspect the laser data and decide what to do int Update( Model* mod, void* dummy ) { - // protect access to this model from other controllers - mod->Lock(); - if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 ) mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), flagsz ) ); - mod->Unlock(); - return 0; // run again } Modified: code/stage/trunk/examples/ctrl/wander.cc =================================================================== --- code/stage/trunk/examples/ctrl/wander.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/examples/ctrl/wander.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -120,7 +120,13 @@ robot->pos->SetXSpeed( cruisespeed ); robot->pos->SetTurnSpeed( 0 ); } - + + if( robot->pos->Stalled() ) + { + robot->pos->SetSpeed( 0,0,0 ); + robot->pos->SetTurnSpeed( 0 ); + } + return 0; } Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/blockgroup.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -58,17 +58,12 @@ Model* BlockGroup::TestCollision() { - //printf( "blockgroup %p test collision...\n", this ); - Model* hitmod = NULL; for( GList* it=blocks; it; it = it->next ) if( (hitmod = ((Block*)it->data)->TestCollision())) break; // bail on the earliest collision - - //printf( "blockgroup %p test collision done.\n", this ); - - return hitmod; + return hitmod; // NULL if no collision } @@ -107,10 +102,6 @@ offset.x = minx + size.x/2.0; offset.y = miny + size.y/2.0; offset.z = 0; // todo? - - // normalize blocks - // for( GList* it = blocks; itl it=it->next ) - //((Block*)it->data)->Normalize( size.x, size.y, size.z, offset.x } @@ -282,11 +273,6 @@ if( rects && (rect_count > 0) ) { - // shift the origin from bottom-left to center of the image - //double dx = width/2.0; - //double dy = height/2.0; - - //puts( "loading rects" ); for( unsigned int r=0; r<rect_count; r++ ) { stg_point_t pts[4]; @@ -296,15 +282,6 @@ double w = rects[r].size.x; double h = rects[r].size.y; -// pts[0].x = x - dx; -// pts[0].y = y - dy; -// pts[1].x = x + w - dx; -// pts[1].y = y -dy; -// pts[2].x = x + w -dx; -// pts[2].y = y + h -dy; -// pts[3].x = x - dx; -// pts[3].y = y + h -dy; - pts[0].x = x; pts[0].y = y; pts[1].x = x + w; @@ -318,10 +295,10 @@ stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); AppendBlock( new Block( mod, - pts,4, - 0,1, - col, - true ) ); + pts,4, + 0,1, + col, + true ) ); } free( rects ); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/model.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -472,7 +472,7 @@ } // convert a global pose into the model's local coordinate system -Pose Model::GlobalToLocal( Pose pose ) +Pose Model::GlobalToLocal( const Pose& pose ) const { // get model's global pose Pose org = GetGlobalPose(); @@ -500,7 +500,7 @@ } // returns true iff model [testmod] is an antecedent of this model -bool Model::IsAntecedent( Model* testmod ) +bool Model::IsAntecedent( const Model* testmod ) const { if( parent == NULL ) return false; @@ -512,7 +512,7 @@ } // returns true iff model [testmod] is a descendent of this model -bool Model::IsDescendent( Model* testmod ) +bool Model::IsDescendent( const Model* testmod ) const { if( this == testmod ) return true; @@ -528,14 +528,14 @@ return false; } -bool Model::IsRelated( Model* that ) +bool Model::IsRelated( const Model* that ) const { // is it me? if( this == that ) return true; // wind up to top-level object - Model* candidate = this; + Model* candidate = (Model*)this; while( candidate->parent ) candidate = candidate->parent; @@ -543,9 +543,7 @@ return candidate->IsDescendent( that ); } - - -inline Pose Model::LocalToGlobal( Pose pose ) +inline Pose Model::LocalToGlobal( const Pose& pose ) const { return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } @@ -612,7 +610,7 @@ pose->a = pose->a; } -void Model::Print( char* prefix ) +void Model::Print( char* prefix ) const { if( prefix ) printf( "%s model ", prefix ); @@ -628,7 +626,7 @@ ((Model*)it->data)->Print( prefix ); } -const char* Model::PrintWithPose() +const char* Model::PrintWithPose() const { Pose gpose = GetGlobalPose(); @@ -678,6 +676,10 @@ void Model::Update( void ) { + // NOTE - update callbacks are NOT called from here, as this method + // may be called in multiple threads, and callbacks may not be + // reentrant + // printf( "[%llu] %s update (%d subs)\n", // this->world->sim_time, this->token, this->subs ); @@ -688,22 +690,8 @@ { // consume energy stored in the power pack stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - pp->Subtract( consumed ); - - /* - printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", - token, - watts, - consumed, - power_pack, - power_pack->stored, - power_pack->capacity, - power_pack->stored / power_pack->capacity * 100.0 ); - */ + pp->Subtract( consumed ); } - - //CallCallbacks( &hooks.update ); - //last_update = world->sim_time; } void Model::CallUpdateCallbacks( void ) @@ -712,7 +700,7 @@ last_update = world->sim_time; } -stg_meters_t Model::ModelHeight() +stg_meters_t Model::ModelHeight() const { stg_meters_t m_child = 0; //max size of any child for( GList* it=this->children; it; it=it->next ) @@ -729,25 +717,20 @@ void Model::AddToPose( double dx, double dy, double dz, double da ) { - if( dx || dy || dz || da ) - { - Pose pose = GetPose(); - pose.x += dx; - pose.y += dy; - pose.z += dz; - pose.a += da; - - SetPose( pose ); - } + Pose pose = this->GetPose(); + pose.x += dx; + pose.y += dy; + pose.z += dz; + pose.a += da; + + this->SetPose( pose ); } -void Model::AddToPose( Pose pose ) +void Model::AddToPose( const Pose& pose ) { this->AddToPose( pose.x, pose.y, pose.z, pose.a ); } - - void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, stg_meters_t ymin, stg_meters_t ymax ) { @@ -836,10 +819,6 @@ Model* Model::ConditionalMove( Pose newpose ) { -// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) -// printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n", -// token, newpose.x, newpose.y, newpose.z, newpose.a ); - Pose startpose = pose; pose = newpose; // do the move provisionally - we might undo it below @@ -853,10 +832,6 @@ world->dirty = true; // need redraw } -// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) -// printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n", -// token, pose.x, pose.y, pose.z, pose.a ); - return hitmod; } @@ -866,12 +841,6 @@ if( disabled ) return; -// if( velocity.IsZero() ) -// { -// PRINT_WARN1( "model %s has velocity zero but its pose is being updated", token ); -// return; -// } - // TODO - control this properly, and maybe do it faster //if( 0 ) //if( (world->updates % 10 == 0) ) @@ -897,9 +866,9 @@ p.z = velocity.z * interval; p.a = velocity.a * interval; - if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) ) - printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n", - token, p.x, p.y, p.z, p.a ); + //if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) ) + //printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n", + // token, p.x, p.y, p.z, p.a ); // attempts to move to the new pose. If the move fails because we'd // hit another model, that model is returned. @@ -909,9 +878,9 @@ } -int Model::TreeToPtrArray( GPtrArray* array ) +int Model::TreeToPtrArray( GPtrArray* array ) const { - g_ptr_array_add( array, this ); + g_ptr_array_add( array, (void*)this ); //printf( " added %s to array at %p\n", root->token, array ); @@ -923,10 +892,10 @@ return added; } -Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) +Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) const { if( (this->type == type) && (this->subs == 0) ) - return this; + return (Model*)this; // discard const // this model is no use. try children recursively for( GList* it = children; it; it=it->next ) @@ -959,7 +928,7 @@ if( (this->type == type) && (!this->used ) ) { this->used = true; - return this; + return this; // discard const } // this model is no use. try children recursively @@ -977,7 +946,7 @@ } -Model* Model::GetModel( const char* modelname ) +Model* Model::GetModel( const char* modelname ) const { // construct the full model name and look it up char* buf = new char[TOKEN_MAX]; @@ -1012,7 +981,7 @@ world->dirty = true; } -PowerPack* Model::FindPowerPack() +PowerPack* Model::FindPowerPack() const { if( power_pack ) return power_pack; Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/model_getset.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -2,11 +2,11 @@ using namespace Stg; -void Model::SetGeom( Geom geom ) +void Model::SetGeom( const Geom& g ) { UnMapWithChildren(); - this->geom = geom; + this->geom = g; blockgroup.CalcSize(); @@ -121,12 +121,12 @@ } // set the pose of model in global coordinates -void Model::SetGlobalPose( Pose gpose ) +void Model::SetGlobalPose( const Pose& gpose ) { SetPose( parent ? parent->GlobalToLocal( gpose ) : gpose ); } -int Model::SetParent( Model* newparent) +int Model::SetParent( Model* newparent) { // remove the model from its old parent (if it has one) if( this->parent ) @@ -143,7 +143,7 @@ } // get the model's velocity in the global frame -Velocity Model::GetGlobalVelocity() +Velocity Model::GetGlobalVelocity() const { Pose gpose = GetGlobalPose(); @@ -159,7 +159,7 @@ } // set the model's velocity in the global frame -void Model::SetGlobalVelocity( Velocity gv ) +void Model::SetGlobalVelocity( const Velocity& gv ) { Pose gpose = GetGlobalPose(); @@ -175,7 +175,7 @@ } // get the model's position in the global frame -Pose Model::GetGlobalPose() +Pose Model::GetGlobalPose() const { // if I'm a top level model, my global pose is my local pose if( parent == NULL ) @@ -191,7 +191,7 @@ } -void Model::SetVelocity( Velocity vel ) +void Model::SetVelocity( const Velocity& vel ) { // assert( ! isnan(vel.x) ); // assert( ! isnan(vel.y) ); @@ -217,7 +217,7 @@ // set the model's pose in the local frame -void Model::SetPose( Pose newpose ) +void Model::SetPose( const Pose& newpose ) { // if the pose has changed, we need to do some work if( memcmp( &pose, &newpose, sizeof(Pose) ) != 0 ) Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/model_position.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -104,10 +104,9 @@ // no power consumed until we're subscribed this->SetWatts( 0 ); - // sensible position defaults - Velocity vel; - memset( &vel, 0, sizeof(vel)); - this->SetVelocity( vel ); + // zero vel + Velocity v; // initially zero + this->SetVelocity( v ); this->SetBlobReturn( TRUE ); @@ -655,7 +654,7 @@ Gl::pose_inverse_shift( pose ); Gl::pose_shift( est_origin ); - glTranslatef( 0,0, 0.02 ); + glTranslatef( 0,0,0.02 ); // draw waypoints for( unsigned int i=0; i < waypoint_count; i++ ) Modified: code/stage/trunk/libstage/model_props.cc =================================================================== --- code/stage/trunk/libstage/model_props.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/model_props.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -4,7 +4,7 @@ #define MATCH(A,B) (strcmp(A,B)== 0) -void* Model::GetProperty( const char* key ) +void* Model::GetProperty( const char* key ) const { // see if the key has the predefined-property prefix if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) @@ -23,7 +23,7 @@ } // otherwise it may be an arbitrary named property - return g_datalist_get_data( &this->props, key ); + return g_datalist_get_data( (GData**)&this->props, key ); // cast to discard const } int Model::SetProperty( const char* key, @@ -94,7 +94,7 @@ } -bool Model::GetPropertyFloat( const char* key, float* f, float defaultval ) +bool Model::GetPropertyFloat( const char* key, float* f, float defaultval ) const { float* fp = (float*)GetProperty( key ); if( fp ) @@ -107,7 +107,7 @@ return false; } -bool Model::GetPropertyInt( const char* key, int* i, int defaultval ) +bool Model::GetPropertyInt( const char* key, int* i, int defaultval ) const { int* ip = (int*)GetProperty( key ); if( ip ) @@ -120,7 +120,7 @@ return false; } -bool Model::GetPropertyStr( const char* key, char** c, char* defaultval ) +bool Model::GetPropertyStr( const char* key, char** c, char* defaultval ) const { char* cp = (char*)GetProperty( key ); Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -34,25 +34,6 @@ const double height = 0.5; const double width = 0.2; - // draw an electric zap -// glPolygonMode( GL_FRONT, GL_LINE ); -// glBegin( GL_POLYGON ); -// glVertex2i( 0, 0 ); -// glVertex2i( 3, 2 ); -// glVertex2i( 1, 2 ); -// glEnd(); - -// glVertex2i( 1, 3 ); -// glVertex2i( 0, 3 ); -// glVertex2i( 1, 5 ); -// glVertex2i( 3, 5 ); -// glVertex2i( 4, 3 ); -// glVertex2i( 5, 3 ); -// glVertex2i( 4, 4 ); -// glVertex2i( 5, 4); -// glEnd(); - //} - double percent = stored/capacity * 100.0; const double alpha = 0.5; @@ -130,7 +111,7 @@ //gl_draw_string( -0.2, 0, 0, buf ); // ? - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + // glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); } @@ -163,7 +144,6 @@ // we can't transfer more than he can take amount = MIN( amount, dest->RemainingCapacity() ); - //printf( "%s gives %.3f J to %s\n", // mod->Token(), amount, dest->mod->Token() ); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/stage.cc 2009-03-28 21:26:54 UTC (rev 7555) @@ -72,10 +72,10 @@ -void Stg::stg_print_velocity( Velocity* vel ) +void Stg::stg_print_velocity( const Velocity& vel ) { printf( "velocity [x:%.3f y:%.3f a:%.3f]\n", - vel->x, vel->y, vel->a ); + vel.x, vel.y, vel.a ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-28 00:10:37 UTC (rev 7554) +++ code/stage/trunk/libstage/stage.hh 2009-03-28 21:26:54 UTC (rev 7555) @@ -153,10 +153,10 @@ /** convert an angle in radians to degrees. */ inline double rtod( double r ){ return( r*180.0/M_PI ); } - + /** convert an angle in degrees to radians. */ inline double dtor( double d ){ return( d*M_PI/180.0 ); } - + /** Normalize an angle to within +/_ M_PI. */ inline double normalize( const double a ) { @@ -280,7 +280,8 @@ prefix, x,y,z,a ); } - bool IsZero(){ return( !(x || y || z || a )); }; + /* returns true iff all components of the velocity are zero. */ + bool IsZero() const { return( !(x || y || z || a )); }; void Load( Worldfile* wf, int section, const char* keyword ); void Save( Worldfile* wf, int section, const char* keyword ); @@ -565,7 +566,7 @@ database, a bright red color (0xF00) will be returned instead. */ stg_color_t stg_lookup_color(const char *name); - + /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */ inline Pose pose_sum( const Pose& p1, const Pose& p2 ) { @@ -590,11 +591,11 @@ /** Report an error, with a standard, friendly message header */ void stg_print_err( const char* err ); /** Print human-readable geometry on stdout */ - void stg_print_geom( Geom* geom ); + void stg_print_geom( const Geom& geom ); /** Print human-readable pose on stdout */ - void stg_print_pose( Pose* pose ); + void stg_print_pose( const Pose& pose ); /** Print human-readable velocity on stdout */ - void stg_print_velocity( Velocity* vel ); + void stg_print_velocity( const Velocity& vel ); /** A model creator function. Each model type must define a function of this type. */ typedef Model* (*stg_creator_t)( World*, Model* ); @@ -975,7 +976,7 @@ Worldfile* GetWorldFile(){ return wf; }; - inline virtual bool IsGUI() { return false; } + virtual bool IsGUI() { return false; } virtual void Load( const char* worldfile_path ); virtual void UnLoad(); @@ -989,7 +990,7 @@ void CancelQuit(){ quit = false; } void CancelQuitAll(){ quit_all = false; } - void TryCharge( PowerPack* pp, Pose pose ); + void TryCharge( PowerPack* pp, const Pose& pose ); /** Get the resolution in pixels-per-metre of the underlying discrete raytracing model */ @@ -1201,13 +1202,13 @@ virtual void Draw( void ) const = 0; virtual void SetProjection( void ) const = 0; - inline float yaw( void ) const { return _yaw; } - inline float pitch( void ) const { return _pitch; } - - inline float x( void ) const { return _x; } - inline float y( void ) const { return _y; } - inline float z( void ) const { return _z; } - + float yaw( void ) const { return _yaw; } + float pitch( void ) const { return _pitch; } + + float x( void ) const { return _x; } + float y( void ) const { return _y; } + float z( void ) const { return _z; } + virtual void reset() = 0; virtual void Load( Worldfile* wf, int sec ) = 0; @@ -1235,34 +1236,34 @@ void strafe( float amount ); void forward( float amount ); - inline void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } - inline void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; } + void setPose( float x, float y, float z ) { _x = x; _y = y; _z = z; } + void addPose( float x, float y, float z ) { _x += x; _y += y; _z += z; if( _z < 0.1 ) _z = 0.1; } void move( float x, float y, float z ); - inline void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; } + void setFov( float horiz_fov, float vert_fov ) { _horiz_fov = horiz_fov; _vert_fov = vert_fov; } ///update vertical fov based on window aspect and current horizontal fov - inline void setAspect( float aspect ) { + void setAspect( float aspect ) { //std::cout << "aspect: " << aspect << " vert: " << _vert_fov << " => " << aspect * _vert_fov << std::endl; //_vert_fov = aspect / _horiz_fov; _aspect = aspect; } - inline void setYaw( float yaw ) { _yaw = yaw; } - inline float horizFov( void ) const { return _horiz_fov; } - inline float vertFov( void ) const { return _vert_fov; } - inline void addYaw( float yaw ) { _yaw += yaw; } - inline void setPitch( float pitch ) { _pitch = pitch; } - inline void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; } + void setYaw( float yaw ) { _yaw = yaw; } + float horizFov( void ) const { return _horiz_fov; } + float vertFov( void ) const { return _vert_fov; } + void addYaw( float yaw ) { _yaw += yaw; } + void setPitch( float pitch ) { _pitch = pitch; } + void addPitch( float pitch ) { _pitch += pitch; if( _pitch < 0 ) _pitch = 0; else if( _pitch > 180 ) _pitch = 180; } - inline float realDistance( float z_buf_val ) const { + float realDistance( float z_buf_val ) const { //formula found at http://www.cs.unc.edu/~hoff/techrep/openglz.html //Z = Zn*Zf / (Zf - z*(Zf-Zn)) return _z_near * _z_far / ( _z_far - z_buf_val * ( _z_far - _z_near ) ); } - inline void scroll( float dy ) { _z += dy; } - inline float nearClip( void ) const { return _z_near; } - inline float farClip( void ) const { return _z_far; } - inline void setClip( float near, float far ) { _z_far = far; _z_near = near; } + void scroll( float dy ) { _z += dy; } + float nearClip( void ) const { return _z_near; } + float farClip( void ) const { return _z_far; } + void setClip( float near, float far ) { _z_far = far; _z_near = near; } - inline void reset() { setPitch( 70 ); setYaw( 0 ); } + void reset() { setPitch( 70 ); setYaw( 0 ); } void Load( Worldfile* wf, int sec ); void Save( Worldfile* wf, int sec ); @@ -1284,10 +1285,10 @@ virtual void SetProjection( void ) const; void move( float x, float y ); - inline void setYaw( float yaw ) { _yaw = yaw; } - inline void setPitch( float pitch ) { _pitch = pitch; } - inline void addYaw( float yaw ) { _yaw += yaw; } - inline void addPitch( float pitch ) { + void setYaw( float yaw ) { _yaw = yaw; } + void setPitch( float pitch ) { _pitch = pitch; } + void addYaw( float yaw ) { _yaw += yaw; } + void addPitch( float pitch ) { _pitch += pitch; if( _pitch > 90 ) _pitch = 90; @@ -1295,13 +1296,13 @@ _pitch = 0; } - inline void setScale( float scale ) { _scale = scale; } - inline void setPose( float x, float y) { _x = x; _y = y; } + void setScale( float scale ) { _scale = scale; } + void setPose( float x, float y) { _x = x; _y = y; } void scale( float scale, float shift_x = 0, float h = 0, float shift_y = 0, float w = 0 ); - inline void reset( void ) { _pitch = _yaw = 0; } + void reset( void ) { _pitch = _yaw = 0; } - inline float scale() const { return _scale; } + float scale() const { return _scale; } void Load( Worldfile* wf, int sec ); void Save( Worldfile* wf, int sec ); @@ -1376,7 +1377,7 @@ virtual void UnLoad(); virtual bool Save( const char* filename ); - inline virtual bool IsGUI() { return true; } + virtual bool IsGUI() { return true; } virtual Model* RecentlySelectedModel(); @@ -1678,7 +1679,7 @@ void MapWithChildren(); void UnMapWithChildren(); - int TreeToPtrArray( GPtrArray* array ); + int TreeToPtrArray( GPtrArray* array ) const; /** raytraces a single ray from the point and heading identified by pose, in local coords */ @@ -1731,7 +1732,7 @@ Model* ConditionalMove( Pose newpose ); - stg_meters_t ModelHeight(); + stg_meters_t ModelHeight() const; bool UpdateDue( void ); void UpdateIfDue(); @@ -1790,7 +1791,7 @@ virtual void PopColor(){ world->PopColor(); } - PowerPack* FindPowerPack(); + PowerPack* FindPowerPack() const; void RecordRenderPoint( GSList** head, GSList* link, unsigned int* c1, unsigned int* c2 ); @@ -1848,7 +1849,7 @@ void PushFlag( Flag* flag ); Flag* PopFlag(); - int GetFlagCount(){ return g_list_length( flag_list ); } + int GetFlagCount() const { return g_list_length( flag_list ); } /** Add a pointer to a blinkenlight to the model. */ void AddBlinkenlight( stg_blinkenlight_t* b ) @@ -1892,33 +1893,33 @@ /** Returns a pointer to this model's parent model, or NULL if this model has no parent */ - Model* Parent(){ return this->parent; } + Model* Parent() const { return this->parent; } - Model* GetModel( const char* name ); + Model* GetModel( const char* name ) const; //int GuiMask(){ return this->gui_mask; }; /** Returns a pointer to the world that contains this model */ - World* GetWorld(){ return this->world; } + World* GetWorld() const { return this->world; } /** return the root model of the tree containing this model */ Model* Root(){ return( parent ? parent->Root() : this ); } - bool IsAntecedent( Model* testmod ); + bool IsAntecedent( const Model* testmod ) const; /** returns true if model [testmod] is a descendent of this model */ - bool IsDescendent( Model* testmod ); + bool IsDescendent( const Model* testmod ) const; /** returns true if model [testmod] is a descendent or antecedent of this model */ - bool IsRelated( Model* testmod ); + bool IsRelated( const Model* testmod ) const; /** get the pose of a model in the global CS */ - Pose GetGlobalPose(); + Pose GetGlobalPose() const; /** get the velocity of a model in the global CS */ - Velocity GetGlobalVelocity(); + Velocity GetGlobalVelocity() const; /* set the velocity of a model in the global coordinate system */ - void SetGlobalVelocity( Velocity gvel ); + void SetGlobalVelocity( const Velocity& gvel ); /** subscribe to a model's data */ void Subscribe(); @@ -1927,54 +1928,55 @@ void Unsubscribe(); /** set the pose of model in global coordinates */ - void SetGlobalPose( Pose gpose ); + void SetGlobalPose( const Pose& gpose ); /** set a model's velocity in its parent's coordinate system */ - void SetVelocity( Velocity vel ); + void SetVelocity( const Velocity& vel ); /** set a model's pose in its parent's coordinate system */ - void SetPose( Pose pose ); + void SetPose( const Pose& pose ); /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( Pose pose ); + void AddToPose( const Pose& pose ); /** add values to a model's pose in its parent's coordinate system */ void AddToPose( double dx, double dy, double dz, double da ); /** set a model's geometry (size and center offsets) */ - void SetGeom( Geom src ); + void SetGeom( const Geom& src ); /** Set a model's fiducial return value. Values less than zero are not detected by the fiducial sensor. */ void SetFiducialReturn( int fid ); /** Get a model's fiducial return value. */ - int GetFiducialReturn() - { return vis.fiducial_return; } + int GetFiducialReturn() const { return vis.fiducial_return; } /** set a model's fiducial key: only fiducial finders with a matching key can detect this model as a fiducial. */ void SetFiducialKey( int key ); - stg_color_t GetColor(){ return color; } + stg_color_t GetColor() const { return color; } /** return a model's unique process-wide identifier */ - uint32_t GetId() { return id; } + uint32_t GetId() const { return id; } // stg_laser_return_t GetLaserReturn(){ return laser_return; } /** Change a model's parent - experimental*/ int SetParent( Model* newparent); - /** Get a model's geometry - it's size and local pose (offset from - origin in local coords) */ - Geom GetGeom(){ return geom; } + /** Get (a copy of) the model's geometry - it's size and local + pose (offset from origin in local coords). */ + Geom GetGeom() const { return geom; } - /** Get the pose of a model in its parent's coordinate system */ - Pose GetPose(){ return pose; } + /** Get (a copy of) the pose of a model in its parent's coordinate + system. */ + Pose GetPose() const { return pose; } - /** Get a model's velocity (in its local reference frame) */ - Velocity GetVelocity(){ return velocity; } + /** Get (a copy of) the model's velocity in its local reference + frame. */ + Velocity GetVelocity() const { return velocity; } // guess what these do? void SetColor( stg_color_t col ); @@ -1993,7 +1995,7 @@ void SetWatts( stg_watts_t watts ); void SetMapResolution( stg_meters_t res ); - bool DataIsFresh(){ return this->data_fresh; } + bool DataIsFresh() const { return this->data_fresh; } /* attach callback functions to data members. The function gets called when the member is changed using SetX() accessor method */ @@ -2042,10 +2044,10 @@ /** named-property interface */ - void* GetProperty( const char* key ); - bool GetPropertyFloat( const char* key, float* f, float defaultval ); - bool GetPropertyInt( const char* key, int* i, int defaultval ); - bool GetPropertyStr( const char* key, char** c, char* defaultval ); + void* GetProperty( const char* key ) const; + bool GetPropertyFloat( const char* key, float* f, float defaultval ) const; + bool GetPropertyInt( const char* key, int* i, int defaultval ) const; + bool GetPropertyStr( const char* key, char** c, char* defaultval ) const; /** @brief Set a named property of a Stage model. @@ -2084,25 +2086,24 @@ void UnsetProperty( const char* key ); - virtual void Print( char* prefix ); - virtual const char* PrintWithPose(); + virtual void Print( char* prefix ) const; + virtual const char* PrintWithPose() const; - /** Convert a pose in the world coordinate system into a model's - local coordinate system. Overwrites [pose] with the new - coordinate. */ - Pose GlobalToLocal( const Pose pose ); - + /** Given a global pose, returns that pose in the model's local + coordinate system. */ + Pose GlobalToLocal( const Pose& pose ) const; + /** Return the global pose (i.e. pose in world coordinates) of a pose specified in the model's local coordinate system */ - Pose LocalToGlobal( const Pose pose ); + Pose LocalToGlobal( const Pose& pose ) const; /** Return the 3d point in world coordinates of a 3d point specified in the model's local coordinate system */ - stg_point3_t LocalToGlobal( const stg_point3_t local ); + stg_point3_t LocalToGlobal( const stg_point3_t local ) const; /** returns the first descendent of this model that is unsubscribed and has the type indicated by the string */ - Model* GetUnsubscribedModelOfType( const stg_model_type_t type ); + Model* GetUnsubscribedModelOfType( const stg_model_type_t type ) const; /** returns the first descendent of this model that is unused and has the type indicated by the string. This model is tagged as used. */ @@ -2110,7 +2111,7 @@ /** Returns the value of the model's stall boolean, which is true iff the model has crashed into another model */ - bool Stalled(){ return this->stall; } + bool Stalled() const { return this->stall; } }; @@ -2568,25 +2569,25 @@ virtual void DataVisualize( Camera* cam ); ///width of captured image - inline int getWidth( void ) const { return _width; } + int getWidth( void ) const { return _width; } ///height of captured image - inline int getHeight( void ) const { return _height; } + int getHeight( void ) const { return _height; } ///get reference to camera used - inline const PerspectiveCamera& getCamera( void ) const { return _camera; } + const PerspectiveCamera& getCamera( void ) const { return _camera; } ///get a reference to camera depth buffer - inline const GLfloat* FrameDepth() const { return _frame_data; } + const GLfloat* FrameDepth() const { return _frame_data; } ///get a reference to camera color image. 3 bytes (RGB) per pixel - inline const GLubyte* FrameColor() const { return _frame_color_data; } + const GLubyte* FrameColor() const { return _frame_color_data; } ///change the pitch - inline void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; } + void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; } ///change the yaw - inline void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; } + void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; } }; // POSITION MODEL -------------------------------------------------------- This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-31 02:48:31
|
Revision: 7562 http://playerstage.svn.sourceforge.net/playerstage/?rev=7562&view=rev Author: rtv Date: 2009-03-31 02:47:55 +0000 (Tue, 31 Mar 2009) Log Message: ----------- added callbacks for flag push and pop, and vis improvements Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/examples/ctrl/sink.cc code/stage/trunk/examples/ctrl/source.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/simple.world Added Paths: ----------- code/stage/trunk/worlds/bitmaps/SFU_1920x1280.png code/stage/trunk/worlds/bitmaps/SFU_640x480.png code/stage/trunk/worlds/bitmaps/SFU_800x600.png code/stage/trunk/worlds/large.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -14,7 +14,7 @@ const int payload = 1; double have[4][4] = { - // { -120, -180, 180, 180 }, + // { -120, -180, 180, 180 } //{ -90, -120, 180, 90 }, { 90, 180, 180, 180 }, { 90, -90, 180, 90 }, @@ -94,6 +94,7 @@ pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); pos->Subscribe(); + // LaserUpdate() controls the robot, by reading from laser and // writing to position laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this ); @@ -110,6 +111,9 @@ blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); blobfinder->Subscribe(); } + + //pos->AddFlagIncrCallback( (stg_model_callback_t)FlagIncr, NULL ); + //pos->AddFlagDecrCallback( (stg_model_callback_t)FlagDecr, NULL ); } void Dock() @@ -480,18 +484,28 @@ return 0; } + + static int FlagIncr( Model* mod, Robot* robot ) + { + printf( "model %s collected flag\n", mod->Token() ); + return 0; + } - + static int FlagDecr( Model* mod, Robot* robot ) + { + printf( "model %s dropped flag\n", mod->Token() ); + return 0; + } }; // Stage calls this when the model starts up extern "C" int Init( Model* mod ) { - Robot* robot = new Robot( (ModelPosition*)mod, - mod->GetWorld()->GetModel( "source" ), - mod->GetWorld()->GetModel( "sink" ) ); - + new Robot( (ModelPosition*)mod, + mod->GetWorld()->GetModel( "source" ), + mod->GetWorld()->GetModel( "sink" ) ); + return 0; //ok } Modified: code/stage/trunk/examples/ctrl/sink.cc =================================================================== --- code/stage/trunk/examples/ctrl/sink.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/examples/ctrl/sink.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -8,11 +8,7 @@ // Stage calls this when the model starts up extern "C" int Init( Model* mod ) { - //for( int i=0; i<5; i++ ) - // mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), 0.5 ) ); - mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); - return 0; //ok } Modified: code/stage/trunk/examples/ctrl/source.cc =================================================================== --- code/stage/trunk/examples/ctrl/source.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/examples/ctrl/source.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -2,19 +2,15 @@ using namespace Stg; const int INTERVAL = 200; +const double FLAGSZ = 0.4; int Update( Model* mod, void* dummy ); -const double flagsz = 0.4; // Stage calls this when the model starts up extern "C" int Init( Model* mod ) { - for( int i=0; i<5; i++ ) - mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0 ), flagsz ) ); - - mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); - + mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); return 0; //ok } @@ -22,7 +18,7 @@ int Update( Model* mod, void* dummy ) { if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 ) - mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), flagsz ) ); + mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), FLAGSZ ) ); return 0; // run again } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/canvas.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -125,7 +125,8 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); // install a font - gl_font( FL_HELVETICA, 12 ); + //gl_font( FL_HELVETICA, 12 ); + gl_font( FL_COURIER, 12 ); blur = false; @@ -955,8 +956,6 @@ glLoadIdentity(); glDisable( GL_DEPTH_TEST ); - // if trails are on, we need to clear the clock background - std::string clockstr = world->ClockString(); if( showFollow == true && last_selection ) clockstr.append( " [ FOLLOW MODE ]" ); @@ -975,18 +974,22 @@ glRectf( 0, 0, width, height ); colorstack.Push( 0,0,0 ); // black Gl::draw_string( margin, margin, 0, clockstr.c_str() ); - - // ENERGY BOX - colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green - glRectf( 0, height, width, 90 ); - colorstack.Push( 0,0,0 ); // black - Gl::draw_string_multiline( margin, height + margin, txtWidth, 50, world->EnergyString().c_str(), (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); - - colorstack.Pop(); colorstack.Pop(); - colorstack.Pop(); colorstack.Pop(); - + + // ENERGY BOX + if( PowerPack::global_capacity > 0 ) + { + colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green + glRectf( 0, height, width, 90 ); + colorstack.Push( 0,0,0 ); // black + Gl::draw_string_multiline( margin, height + margin, txtWidth, 50, + world->EnergyString().c_str(), + (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); + colorstack.Pop(); + colorstack.Pop(); + } + glEnable( GL_DEPTH_TEST ); glPopMatrix(); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/model.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -197,7 +197,7 @@ blockgroup(), blocks_dl(0), boundary(false), - callbacks( g_hash_table_new( g_int_hash, g_int_equal ) ), + callbacks( g_hash_table_new( g_direct_hash, g_direct_equal ) ), color( 0xFFFF0000 ), // red data_fresh(false), disabled(false), @@ -327,19 +327,28 @@ void Model::AddFlag( Flag* flag ) { if( flag ) - flag_list = g_list_append( this->flag_list, flag ); + { + flag_list = g_list_append( this->flag_list, flag ); + CallCallbacks( &hooks.flag_incr ); + } } void Model::RemoveFlag( Flag* flag ) { if( flag ) - flag_list = g_list_remove( this->flag_list, flag ); + { + flag_list = g_list_remove( this->flag_list, flag ); + CallCallbacks( &hooks.flag_decr ); + } } void Model::PushFlag( Flag* flag ) { if( flag ) - flag_list = g_list_prepend( flag_list, flag); + { + flag_list = g_list_prepend( flag_list, flag); + CallCallbacks( &hooks.flag_incr ); + } } Flag* Model::PopFlag() @@ -347,9 +356,13 @@ if( flag_list == NULL ) return NULL; + printf( "pop flag" ); + Flag* flag = (Flag*)flag_list->data; flag_list = g_list_remove( flag_list, flag ); + CallCallbacks( &hooks.flag_decr ); + return flag; } Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -1,19 +1,19 @@ #include "stage.hh" using namespace Stg; -int key_gen( Model* mod, void* address ) -{ - return ((int*)address) - ((int*)mod); -} +// int key_gen( Model* mod, void* address ) +// { +// return ((int*)address) - ((int*)mod); +// } void Model::AddCallback( void* address, stg_model_callback_t cb, void* user ) { - int* key = (int*)g_new( int, 1 ); - *key = key_gen( this, address ); + ///int* key = (int*)g_new( int, 1 ); + //*key = key_gen( this, address ); - GList* cb_list = (GList*)g_hash_table_lookup( callbacks, key ); + GList* cb_list = (GList*)g_hash_table_lookup( callbacks, address ); //printf( "installing callback in model %s with key %d\n", // mod->token, *key ); @@ -22,16 +22,16 @@ cb_list = g_list_prepend( cb_list, cb_create( cb, user ) ); // and replace the list in the hash table - g_hash_table_insert( callbacks, key, cb_list ); + g_hash_table_insert( callbacks, address, cb_list ); } int Model::RemoveCallback( void* member, stg_model_callback_t callback ) { - int key = key_gen( this, member ); + //int key = key_gen( this, member ); - GList* cb_list = (GList*)g_hash_table_lookup( callbacks, &key ); + GList* cb_list = (GList*)g_hash_table_lookup( callbacks, member ); // find our callback in the list of stg_cbarg_t GList* el = NULL; @@ -52,7 +52,7 @@ cb_list = g_list_remove( cb_list, el->data); // store the new, shorter, list of callbacks - g_hash_table_insert( callbacks, &key, cb_list ); + g_hash_table_insert( callbacks, member, cb_list ); // we're done with that //free( el->data ); @@ -76,28 +76,32 @@ void Model::CallCallbacks( void* address ) { + assert( address ); - int key = key_gen( this, address ); + //int key = key_gen( this, address ); + + //printf( "CallCallbacks for model %s %p key %p\n", this->Token(), this, address ); //printf( "Model %s has %d callbacks. Checking key %d\n", - // mod->token, g_hash_table_size( mod->callbacks ), key ); + // this->token, g_hash_table_size( this->callbacks ), key ); - GList* cbs = (GList*)g_hash_table_lookup( callbacks, &key ); + GList* cbs = (GList*)g_hash_table_lookup( callbacks, address ); //printf( "key %d has %d callbacks registered\n", - // key, g_list_length( cbs ) ); - + // key, g_list_length( cbs ) ); + // maintain a list of callbacks that should be cancelled GList* doomed = NULL; // for each callback in the list while( cbs ) { + //printf( "cbs %p data %p cvs->next %p\n", cbs, cbs->data, cbs->next ); + stg_cb_t* cba = (stg_cb_t*)cbs->data; + assert( cba ); - //puts( "calling..." ); - if( (cba->callback)( this, cba->arg ) ) { //printf( "callback returned TRUE - schedule removal from list\n" ); @@ -121,6 +125,7 @@ g_list_free( doomed ); } + //puts( "Callbacks done" ); } Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/model_getset.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -212,7 +212,7 @@ on_velocity_list = true; } - CallCallbacks( &this->velocity ); + //CallCallbacks( &this->velocity ); } Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -263,7 +263,7 @@ Gl::pose_inverse_shift( mod->GetPose() ); - glTranslatef( -width/2.0, -height/2.0, 0 ); + glTranslatef( -width/2.0, -height/2.0, 0.01 ); glScalef( cellsize, cellsize, 1 ); for( unsigned int y=0; y<rows; y++ ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/stage.hh 2009-03-31 02:47:55 UTC (rev 7562) @@ -1432,8 +1432,8 @@ /** energy data packet */ class PowerPack { - //friend class World; friend class WorldGui; + friend class Canvas; protected: @@ -1554,11 +1554,13 @@ class CallbackHooks { public: - char load; - char save; - char shutdown; - char startup; - char update; + int flag_incr; + int flag_decr; + int load; + int save; + int shutdown; + int startup; + int update; }; /** Records model state and functionality in the GUI, if used */ @@ -2105,7 +2107,19 @@ void RemoveUpdateCallback( stg_model_callback_t cb ) { RemoveCallback( &hooks.update, cb ); } + + void AddFlagIncrCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.flag_incr, cb, user ); } + void RemoveFlagIncrCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.flag_incr, cb ); } + + void AddFlagDecrCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.flag_decr, cb, user ); } + + void RemoveFlagDecrCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.flag_decr, cb ); } + /** named-property interface */ void* GetProperty( const char* key ) const; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/world.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -675,18 +675,6 @@ else if( dy > 0 ) n++; - if( abs(sup.x) > 20 ) - printf( "raytracing at [ %.2f %.2f %.2f %.2f ] GLOB( %d %d ) SUP( %d %d )\n", - gpose.x, - gpose.y, - gpose.z, - gpose.a, - glob.x, - glob.y, - sup.x, - sup.y ); - - // find the starting superregion sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-31 02:47:55 UTC (rev 7562) @@ -416,7 +416,7 @@ str += buf; } - snprintf( buf, 255, "%um%02us%03umsec [%.2f]", minutes, seconds, msec, localratio ); + snprintf( buf, 255, " %um %02us %03umsec [%.2f]", minutes, seconds, msec, localratio ); str += buf; if( paused == true ) Added: code/stage/trunk/worlds/bitmaps/SFU_1920x1280.png =================================================================== (Binary files differ) Property changes on: code/stage/trunk/worlds/bitmaps/SFU_1920x1280.png ___________________________________________________________________ Added: svn:mime-type + image/png Added: code/stage/trunk/worlds/bitmaps/SFU_640x480.png =================================================================== (Binary files differ) Property changes on: code/stage/trunk/worlds/bitmaps/SFU_640x480.png ___________________________________________________________________ Added: svn:mime-type + image/png Added: code/stage/trunk/worlds/bitmaps/SFU_800x600.png =================================================================== (Binary files differ) Property changes on: code/stage/trunk/worlds/bitmaps/SFU_800x600.png ___________________________________________________________________ Added: svn:mime-type + image/png Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/worlds/fasr.world 2009-03-31 02:47:55 UTC (rev 7562) @@ -8,13 +8,13 @@ interval_sim 100 # simulation timestep in milliseconds interval_real 0 # real-time interval between simulation updates in milliseconds -paused 0 +paused 1 resolution 0.02 # threads may speed things up here depending on available CPU cores & workload - threadpool 8 -# threadpool 0 +# threadpool 8 + threadpool 0 # configure the GUI window @@ -22,9 +22,9 @@ ( size [ 788.000 842.000 ] - center [ 0.240 -0.382 ] + center [ 0.121 -0.854 ] rotate [ 0 0 ] - scale 35.648 + scale 42.424 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -153,23 +153,23 @@ #puck( pose [ 1.067 3.367 0 0 ] ) #puck( pose [ 1.412 3.604 0 0 ] ) -autorob( pose [5.418 7.478 0 -163.478] joules 300000 ) -autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) +autorob( pose [5.488 5.149 0 35.947] joules 300000 ) +autorob( pose [6.431 5.593 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) autorob( pose [7.394 5.595 0 129.497] joules 400000 ) -autorob( pose [6.468 6.708 0 170.743] joules 100000 ) +autorob( pose [5.422 7.030 0 170.743] joules 100000 ) autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) -autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) -autorob( pose [4.161 5.544 0 -147.713] joules 400000 ) -autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) -autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) -autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) -autorob( pose [6.362 5.632 0 163.239] joules 400000 db_count 3 db [ "foo<int>42" "bar<float>6.75" "bash<string>my old shoe" ] ) +#autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) +#autorob( pose [4.161 5.544 0 -147.713] joules 400000 ) +#autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) +#autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) +#autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) +#autorob( pose [6.362 5.632 0 163.239] joules 400000 ) #autorob( pose [7.559 4.764 0 -139.066] ) #autorob( pose [5.471 7.446 0 77.301] ) Added: code/stage/trunk/worlds/large.world =================================================================== --- code/stage/trunk/worlds/large.world (rev 0) +++ code/stage/trunk/worlds/large.world 2009-03-31 02:47:55 UTC (rev 7562) @@ -0,0 +1,60 @@ +# simple.world - basic world file example +# Authors: Richard Vaughan +# $Id$ + +include "pioneer.inc" +include "map.inc" +include "sick.inc" + +interval_sim 100 # simulation timestep in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds + +paused 1 + +resolution 0.02 + +# configure the GUI window +window +( + size [ 556.000 557.000 ] # in pixels + scale 28.116 + # pixels per meter + center [ 8.058 7.757 ] + rotate [ 0 0 ] + + show_data 1 # 1=on 0=off + show_grid 0 +) + +# load an environment bitmap +floorplan +( + name "cave" + size [1000.000 700.000 0.600] + pose [0 0 0 0] + bitmap "bitmaps/SFU_800x600.png" +) + + +define wanderer pioneer2dx +( + sicklaser( size [0.3 0.3 0.2 ] samples 32 ) + ctrl "wander" + + # report error-free position in world coordinates + localization "gps" + localization_origin [ 0 0 0 0 ] +) + +wanderer( pose [ 0 0 0 0 ] ) +wanderer( pose [ 1 0 0 0 ] ) +wanderer( pose [ 2 0 0 0 ] ) +wanderer( pose [ 3 0 0 0 ] ) +wanderer( pose [ 4 0 0 0 ] ) +wanderer( pose [ 5 0 0 0 ] ) +wanderer( pose [ 6 0 0 0 ] ) +wanderer( pose [ 7 0 0 0 ] ) +wanderer( pose [ 8 0 0 0 ] ) +wanderer( pose [ 9 0 0 0 ] ) + + Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-03-30 21:43:22 UTC (rev 7561) +++ code/stage/trunk/worlds/simple.world 2009-03-31 02:47:55 UTC (rev 7562) @@ -29,9 +29,9 @@ floorplan ( name "cave" - size [16.000 16.000 0.800] - pose [8 8 0 0] - bitmap "bitmaps/cave.png" + size [160.000 160.000 0.800] + pose [0 0 0 0] + bitmap "bitmaps/SFU_800x600.png" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-02 00:06:52
|
Revision: 7569 http://playerstage.svn.sourceforge.net/playerstage/?rev=7569&view=rev Author: rtv Date: 2009-04-02 00:06:20 +0000 (Thu, 02 Apr 2009) Log Message: ----------- correctly normalizing angles Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-31 22:46:27 UTC (rev 7568) +++ code/stage/trunk/libstage/model.cc 2009-04-02 00:06:20 UTC (rev 7569) @@ -829,8 +829,11 @@ blockgroup.SwitchToTestedCells(); } -Model* Model::ConditionalMove( Pose newpose ) +Model* Model::ConditionalMove( const Pose& newpose ) { + assert( newpose.a >= -M_PI ); + assert( newpose.a <= M_PI ); + Pose startpose = pose; pose = newpose; // do the move provisionally - we might undo it below @@ -876,7 +879,7 @@ p.x = velocity.x * interval; p.y = velocity.y * interval; p.z = velocity.z * interval; - p.a = velocity.a * interval; + p.a = normalize( velocity.a * interval ); //if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) ) //printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n", @@ -884,8 +887,12 @@ // attempts to move to the new pose. If the move fails because we'd // hit another model, that model is returned. - Model* hitthing = ConditionalMove( pose_sum( pose, p ) ); + Pose q = pose_sum( pose, p ); + assert( q.a >= -M_PI ); + assert( q.a <= M_PI ); + Model* hitthing = ConditionalMove( q ); + SetStall( hitthing ? true : false ); } Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-31 22:46:27 UTC (rev 7568) +++ code/stage/trunk/libstage/model_position.cc 2009-04-02 00:06:20 UTC (rev 7569) @@ -561,7 +561,7 @@ // figure out where the implied origin is in global coords Pose gp = GetGlobalPose(); - double da = -odom.a + gp.a; + double da = normalize( -odom.a + gp.a ); double dx = -odom.x * cos(da) + odom.y * sin(da); double dy = -odom.y * cos(da) - odom.x * sin(da); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-31 22:46:27 UTC (rev 7568) +++ code/stage/trunk/libstage/stage.hh 2009-04-02 00:06:20 UTC (rev 7569) @@ -158,11 +158,11 @@ inline double dtor( double d ){ return( d*M_PI/180.0 ); } /** Normalize an angle to within +/_ M_PI. */ - inline double normalize( const double a ) + inline double normalize( double a ) { - static const double TWO_PI = 2.0 * M_PI; - assert( ! isnan(a) ); - return( fmod(a + M_PI, TWO_PI ) - M_PI); + while( a < -M_PI ) a += 2.0*M_PI; + while( a > M_PI ) a -= 2.0*M_PI; + return a; }; /** take binary sign of a, either -1, or 1 if >= 0 */ @@ -1827,7 +1827,7 @@ void StartUpdating(); void StopUpdating(); - Model* ConditionalMove( Pose newpose ); + Model* ConditionalMove( const Pose& newpose ); stg_meters_t ModelHeight() const; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-31 22:46:27 UTC (rev 7568) +++ code/stage/trunk/libstage/world.cc 2009-04-02 00:06:20 UTC (rev 7569) @@ -527,26 +527,23 @@ void World::Raytrace( const Pose &pose, // global pose - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const Model* model, - const void* arg, - stg_raytrace_result_t* samples, // preallocated storage for samples - const uint32_t sample_count, - const bool ztest ) // number of samples + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const Model* model, + const void* arg, + stg_raytrace_result_t* samples, // preallocated storage for samples + const uint32_t sample_count, // number of samples + const bool ztest ) { // find the direction of the first ray Pose raypose = pose; - raypose.a -= fov/2.0; - - // increment the ray direction by this much for each sample - stg_radians_t angle_incr = fov/(double)sample_count; - + double starta = fov/2.0; + for( uint32_t s=0; s < sample_count; s++ ) { + raypose.a = s * fov / (double)sample_count) - starta; samples[s] = Raytrace( raypose, range, func, model, arg, ztest ); - raypose.a += angle_incr; } } @@ -613,6 +610,7 @@ // initialize the sample sample.pose = gpose; + sample.pose.a = normalize( sample.pose.a ); sample.range = range; // we might change this below sample.mod = NULL; // we might change this below Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-03-31 22:46:27 UTC (rev 7568) +++ code/stage/trunk/worlds/simple.world 2009-04-02 00:06:20 UTC (rev 7569) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 20 # real-time interval between simulation updates in milliseconds paused 0 @@ -19,7 +19,7 @@ size [ 556.000 557.000 ] # in pixels scale 28.116 # pixels per meter - center [ 8.058 7.757 ] + center [ 0.095 -0.301 ] rotate [ 0 0 ] show_data 1 # 1=on 0=off @@ -29,9 +29,9 @@ floorplan ( name "cave" - size [160.000 160.000 0.800] + size [16.000 16.000 0.800] pose [0 0 0 0] - bitmap "bitmaps/SFU_800x600.png" + bitmap "bitmaps/cave.png" ) @@ -40,7 +40,7 @@ # can refer to the robot by this name name "r0" - pose [ 0.892 0.800 0 56.500 ] + pose [ -7 -7 0 45 ] sicklaser() ctrl "wander" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-02 23:51:44
|
Revision: 7571 http://playerstage.svn.sourceforge.net/playerstage/?rev=7571&view=rev Author: rtv Date: 2009-04-02 23:50:59 +0000 (Thu, 02 Apr 2009) Log Message: ----------- cleaned up cmdline args Modified Paths: -------------- code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-04-02 01:04:09 UTC (rev 7570) +++ code/stage/trunk/libstage/main.cc 2009-04-02 23:50:59 UTC (rev 7571) @@ -10,12 +10,20 @@ #include "config.h" using namespace Stg; +const char* USAGE = + "USAGE: stage [options] [<worldfile>]\n" + "Available [options] are:\n" + " --gui : run without a GUI (same as -g)\n" + " -g : run without a GUI (same as --gui)\n" + " --help : print this message.\n" + " -h : print this message.\n" + " -? : print this message.\n" + " If <worldfile> is not specified, Stage starts with a file selector dialog"; + /* options descriptor */ static struct option longopts[] = { { "gui", optional_argument, NULL, 'g' }, - { "port", required_argument, NULL, 'p' }, - { "host", required_argument, NULL, 'h' }, - { "federation", required_argument, NULL, 'f' }, + { "help", optional_argument, NULL, 'h' }, { NULL, 0, NULL, 0 } }; @@ -29,7 +37,7 @@ int ch=0, optindex=0; bool usegui = true; - while ((ch = getopt_long(argc, argv, "gfp:h:f:", longopts, &optindex)) != -1) + while ((ch = getopt_long(argc, argv, "gh?", longopts, &optindex)) != -1) { switch( ch ) { @@ -40,12 +48,15 @@ usegui = false; printf( "[GUI disabled]" ); break; - case 'p': - printf( "PORT %d\n", atoi(optarg) ); + case 'h': case '?': + puts( USAGE ); + exit(0); break; default: printf("unhandled option %c\n", ch ); + puts( USAGE ); + exit(0); } } @@ -76,17 +87,10 @@ new WorldGui( 400, 300 ); } - if( usegui == true ) - { - //don't close the window once time has finished - while( true ) - World::UpdateAll(); - } - else - { - //close program once time has completed - bool quit = false; - while( quit == false ) - quit = World::UpdateAll(); - } + if( usegui ) + while( true ) World::UpdateAll(); + else + while( ! World::UpdateAll() ); + + exit(0); } Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-04-02 01:04:09 UTC (rev 7570) +++ code/stage/trunk/libstage/model_draw.cc 2009-04-02 23:50:59 UTC (rev 7571) @@ -247,15 +247,12 @@ void Model::AddVisualizer( Visualizer* custom_visual, bool on_by_default ) { - if( !custom_visual ) - return; - - //Visualizations can only be added to stage when run in a GUI - if( world_gui == NULL ) - { - printf( "Unable to add custom visualization - it must be run with a GUI world\n" ); - return; - } + if( !custom_visual ) + return; + + // If there's no GUI, ignore this request + if( ! world_gui ) + return; //save visual instance custom_visual_list = g_list_append(custom_visual_list, custom_visual ); Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-04-02 01:04:09 UTC (rev 7570) +++ code/stage/trunk/libstage/powerpack.cc 2009-04-02 23:50:59 UTC (rev 7571) @@ -228,7 +228,6 @@ output_vis.AppendValue( amount ); stored_vis.AppendValue( stored ); - //stg_watts_t w = j / (interval / 1e6); } void PowerPack::Dissipate( stg_joules_t j, const Pose& p ) Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-04-02 01:04:09 UTC (rev 7570) +++ code/stage/trunk/worlds/simple.world 2009-04-02 23:50:59 UTC (rev 7571) @@ -9,6 +9,8 @@ interval_sim 100 # simulation timestep in milliseconds interval_real 20 # real-time interval between simulation updates in milliseconds +quit_time 60 + paused 0 resolution 0.02 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-04 00:17:55
|
Revision: 7576 http://playerstage.svn.sourceforge.net/playerstage/?rev=7576&view=rev Author: rtv Date: 2009-04-04 00:17:35 +0000 (Sat, 04 Apr 2009) Log Message: ----------- added model rasterization Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-04 00:17:35 UTC (rev 7576) @@ -502,6 +502,24 @@ // Stage calls this when the model starts up extern "C" int Init( Model* mod ) { + if( strcmp( mod->Token(), "r0" ) == 0 ) + { + const unsigned int dw = 60, dh = 30; + uint8_t* data = new uint8_t[dw*dh*2]; + memset( data, 0, sizeof(uint8_t) * dw * dh ); + + mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh ); + + putchar( '\n' ); + for( unsigned int y=0; y<dh; y++ ) + { + for( unsigned int x=0; x<dw; x++ ) + putchar( data[x + ((dh-y-1)*dw)] ? 'O' : '.' ); + putchar( '\n' ); + } + delete data; + } + new Robot( (ModelPosition*)mod, mod->GetWorld()->GetModel( "source" ), mod->GetWorld()->GetModel( "sink" ) ); Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/libstage/block.cc 2009-04-04 00:17:35 UTC (rev 7576) @@ -314,7 +314,88 @@ mapped = true; } +void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +{ + Pose pose;// = mod->GetPose(); + + // add local offset + pose = pose_sum( pose, mod->geom.pose ); + + Size bgsize = mod->blockgroup.GetSize(); + double scalex = (width-1) / bgsize.x; + double scaley = (height-1) / bgsize.y; + + Rasterize( data, width, height, scalex, scaley, 0,0 ); +} + +void swap( int& a, int& b ) +{ + int tmp = a; + a = b; + b = tmp; +} + +void Block::Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + double scalex, double scaley, + double offsetx, double offsety ) +{ + //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", + // this, width, height, scalex, scaley, offsetx, offsety ); + + for( unsigned int p=0; p<pt_count; p++ ) + { + int xa = round( (pts[p ].x + offsetx) * scalex ); + int ya = round( (pts[p ].y + offsety) * scaley ); + int xb = round( (pts[(p+1)%pt_count].x + offsetx) * scalex ); + int yb = round( (pts[(p+1)%pt_count].y + offsety) * scaley ); + + //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); + + bool steep = abs( yb-ya ) > abs( xb-xa ); + if( steep ) + { + swap( xa, ya ); + swap( xb, yb ); + } + + if( xa > xb ) + { + swap( xa, xb ); + swap( ya, yb ); + } + + int x; + float dydx = (float) (yb - ya) / (float) (xb - xa); + float y = ya; + for (x=xa; x<=xb; x++) + { + if( steep ) + { + if( ! (round(y) >= 0) ) continue; + if( ! (round(y) < (int)width) ) continue; + if( ! (x >= 0) ) continue; + if( ! (x < height) ) continue; + } + else + { + if( ! (x >= 0) ) continue; + if( ! (x < (int)width) ) continue; + if( ! (round(y) >= 0) ) continue; + if( ! (round(y) < height) ) continue; + } + + if( steep ) + data[ (int)round(y) + (x * width)] = 1; + else + data[ x + ((int)round(y) * width)] = 1; + y = y + dydx; + } + } +} + + void Block::DrawTop() { // draw the top of the block - a polygon at the highest vertical Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-04 00:17:35 UTC (rev 7576) @@ -305,3 +305,15 @@ CalcSize(); } + + +#include <math.h> /* for round() */ +extern void output (int x, int y); /* forward declaration for user-defined output */ + + + +void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +{ + for( GList* it = blocks; it; it=it->next ) + ((Block*)it->data)->Rasterize( data, width, height ); +} Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/libstage/model.cc 2009-04-04 00:17:35 UTC (rev 7576) @@ -1020,3 +1020,7 @@ } +void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +{ + blockgroup.Rasterize( data, width, height ); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/libstage/stage.hh 2009-04-04 00:17:35 UTC (rev 7576) @@ -1121,6 +1121,14 @@ stg_color_t GetColor(); + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + double scalex, double scaley, + double offsetx, double offsety ); + + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height ); + private: Model* mod; ///< model to which this block belongs @@ -1187,9 +1195,8 @@ void CallDisplayList( Model* mod ); void Clear() ; /** deletes all blocks from the group */ - GList* AppendTouchingModels( GList* list ); - //void AddTouchingModelsToList( GList* list ); - + GList* AppendTouchingModels( GList* list ); + /** Returns a pointer to the first model detected to be colliding with a block in this group, or NULL, if none are detected. */ Model* TestCollision(); @@ -1207,6 +1214,8 @@ void LoadBitmap( Model* mod, const char* bitmapfile, Worldfile *wf ); void LoadBlock( Model* mod, Worldfile* wf, int entity ); + + void Rasterize( uint8_t* data, unsigned int width, unsigned int height ); }; @@ -1722,6 +1731,10 @@ Visibility vis; stg_usec_t GetSimInterval(){ return world->interval_sim; } + + /** Render the model's blocks as an occupancy grid into the + preallocated array of width by height pixels */ + void Rasterize( uint8_t* data, unsigned int width, unsigned int height ); void Lock() { Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-04-03 19:03:02 UTC (rev 7575) +++ code/stage/trunk/worlds/fasr.world 2009-04-04 00:17:35 UTC (rev 7576) @@ -153,7 +153,7 @@ #puck( pose [ 1.067 3.367 0 0 ] ) #puck( pose [ 1.412 3.604 0 0 ] ) -autorob( pose [5.488 5.149 0 35.947] joules 300000 ) +autorob( pose [5.488 5.149 0 35.947] joules 300000 name "r0" ) autorob( pose [6.431 5.593 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-07 02:01:25
|
Revision: 7578 http://playerstage.svn.sourceforge.net/playerstage/?rev=7578&view=rev Author: rtv Date: 2009-04-07 01:35:45 +0000 (Tue, 07 Apr 2009) Log Message: ----------- debugging rasterization Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/bitmaps/SFU_1920x1280.png Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-07 01:35:45 UTC (rev 7578) @@ -504,7 +504,7 @@ { if( strcmp( mod->Token(), "r0" ) == 0 ) { - const unsigned int dw = 60, dh = 30; + const unsigned int dw = 61, dh = 31; uint8_t* data = new uint8_t[dw*dh*2]; memset( data, 0, sizeof(uint8_t) * dw * dh ); Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/libstage/block.cc 2009-04-07 01:35:45 UTC (rev 7578) @@ -316,24 +316,31 @@ void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) { - Pose pose;// = mod->GetPose(); - // add local offset - pose = pose_sum( pose, mod->geom.pose ); + // pose = pose_sum( pose, mod->geom.pose ); Size bgsize = mod->blockgroup.GetSize(); - double scalex = (width-1) / bgsize.x; - double scaley = (height-1) / bgsize.y; + double scalex = (double)(width) / (double)bgsize.x; + double scaley = (double)(height) / (double)bgsize.y; + //double scalex = (width) / bgsize.x; + //double scaley = (height) / bgsize.y; Rasterize( data, width, height, scalex, scaley, 0,0 ); } -void swap( int& a, int& b ) +// void swap( int& a, int& b ) +// { +// int tmp = a; +// a = b; +// b = tmp; +// } + +void swap( int* a, int* b ) { - int tmp = a; - a = b; - b = tmp; + int foo = *a; + *a = *b; + *b = foo; } void Block::Rasterize( uint8_t* data, @@ -344,53 +351,86 @@ //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", // this, width, height, scalex, scaley, offsetx, offsety ); - for( unsigned int p=0; p<pt_count; p++ ) + unsigned int W=0; + +// W+=20; +// W /= 2; + +// printf( "W is %u", W ); + + for( W=0; W<pt_count; W++ ) { - int xa = round( (pts[p ].x + offsetx) * scalex ); - int ya = round( (pts[p ].y + offsety) * scaley ); - int xb = round( (pts[(p+1)%pt_count].x + offsetx) * scalex ); - int yb = round( (pts[(p+1)%pt_count].y + offsety) * scaley ); + double px = pts[W ].x; + double py = pts[(W+1)%pt_count].x; + unsigned int keep_W = W; + int xa = floor( (pts[W ].x + offsetx) * scalex ); + int ya = floor( (pts[W ].y + offsety) * scaley ); + int xb = floor( (pts[(W+1)%pt_count].x + offsetx) * scalex ); + int yb = floor( (pts[(W+1)%pt_count].y + offsety) * scaley ); + + int keep_xa = xa; + int keep_xb = xb; + + //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); bool steep = abs( yb-ya ) > abs( xb-xa ); - if( steep ) + if( steep ) + { + swap( &xa, &ya ); + swap( &xb, &yb ); + } + + if( xa > xb ) { - swap( xa, ya ); - swap( xb, yb ); + swap( &xa, &xb ); + swap( &ya, &yb ); } - if( xa > xb ) - { - swap( xa, xb ); - swap( ya, yb ); - } - - int x; - float dydx = (float) (yb - ya) / (float) (xb - xa); - float y = ya; - for (x=xa; x<=xb; x++) + double dydx = (double) (yb - ya) / (double) (xb - xa); + double y = ya; + for(int x=xa; x<=xb; x++) { - if( steep ) - { - if( ! (round(y) >= 0) ) continue; - if( ! (round(y) < (int)width) ) continue; - if( ! (x >= 0) ) continue; - if( ! (x < height) ) continue; - } - else - { - if( ! (x >= 0) ) continue; - if( ! (x < (int)width) ) continue; - if( ! (round(y) >= 0) ) continue; - if( ! (round(y) < height) ) continue; - } + // if( steep ) +// { +// if( ! (floor(y) >= 0) ) continue; +// if( ! (floor(y) < (int)width) ) continue; +// if( ! (x >= 0) ) continue; +// if( ! (x < (int)height) ) continue; +// } +// else +// { +// if( ! (x >= 0) ) continue; +// if( ! (x < (int)width) ) continue; +// if( ! (floor(y) >= 0) ) continue; +// if( ! (floor(y) < (int)height) ) continue; +// } if( steep ) - data[ (int)round(y) + (x * width)] = 1; + data[ (int)floor(y) + (x * width)] = 1; else - data[ x + ((int)round(y) * width)] = 1; - y = y + dydx; + data[ x + ((int)floor(y) * width)] = 1; + y += dydx; + +// if( (floor(y) == 75) && +// x == 119 ) +// { +// puts( "foo" ); +// // while(1) {} + +// printf( "W: %u keep_W: %u px: %.4f\npy: %.4f\n", +// W, keep_W, +// px, py ); + +// printf( "XA: %.4f\nXB: %.4f\n", +// (pts[W ].x + offsetx) * scalex, +// (pts[(W+1)%pt_count].x + offsetx) * scalex ); + +// printf( "KEEP: %d %d\n", keep_xa, keep_xb ); +// printf( "NOW: %d %d\n", xa, xb ); +// } + } } } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/libstage/model.cc 2009-04-07 01:35:45 UTC (rev 7578) @@ -218,6 +218,7 @@ power_pack( NULL ), pps_charging(NULL), props(NULL), + rastervis(), rebuild_displaylist(true), say_string(NULL), stall(false), @@ -263,6 +264,8 @@ // now we can add the basic square shape AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 ); + AddVisualizer( &rastervis, true ); + PRINT_DEBUG2( "finished model %s @ %p", this->token, this ); } @@ -1023,4 +1026,91 @@ void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) { blockgroup.Rasterize( data, width, height ); + rastervis.SetData( data, width, height ); } + +//*************************************************************** +// Raster data visualizer +// +Model::RasterVis::RasterVis() + : Visualizer( "Rasterization", "raster_vis" ), + data(NULL), + width(0), + height(0) +{ +} + +void Model::RasterVis::Visualize( Model* mod, Camera* cam ) +{ + if( data == NULL ) + return; + + // go into world coordinates + glPushMatrix(); + mod->PushColor( 0,0,0,0.5 ); + + Gl::pose_inverse_shift( mod->GetGlobalPose() ); + + glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); + glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + + glPolygonMode( GL_FRONT, GL_FILL ); + for( unsigned int y=0; y<height; y++ ) + for( unsigned int x=0; x<width; x++ ) + { + // printf( "[%u %u] ", x, y ); + + if( (x == (92/5)) && (y == (750/10) )) + { + mod->PushColor( 1,0,0,0.5 ); + glRectf( x, y, x+1, y+1 ); + mod->PopColor(); + } + else if( data[ x + y*width ] ) + glRectf( x, y, x+1, y+1 ); + } + + glTranslatef( 0,0,0.01 ); + + mod->PushColor( 0,0,0,1 ); + glPolygonMode( GL_FRONT, GL_LINE ); + for( unsigned int y=0; y<height; y++ ) + for( unsigned int x=0; x<width; x++ ) + { + //if( data[ x + y*width ] ) + glRectf( x, y, x+1, y+1 ); + + char buf[128]; + snprintf( buf, 127, "[%u x %u]", x, y ); + Gl::draw_string( x, y, 0, buf ); + } + + + glPolygonMode( GL_FRONT, GL_FILL ); + + mod->PopColor(); + mod->PopColor(); + + mod->PushColor( 0,0,0,1 ); + char buf[128]; + snprintf( buf, 127, "[%u x %u]", width, height ); + glTranslatef( 0,0,0.01 ); + Gl::draw_string( 1, height-1, 0, buf ); + mod->PopColor(); + + glPopMatrix(); +} + +void Model::RasterVis::SetData( uint8_t* data, unsigned int width, unsigned int height ) +{ + // copy the raster for test visualization + if( this->data ) + free( this->data ); + size_t len = sizeof(uint8_t) * width * height; + this->data = (uint8_t*)malloc( len ); + memcpy( this->data, data, len ); + this->width = width; + this->height = height; +} + + Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/libstage/powerpack.cc 2009-04-07 01:35:45 UTC (rev 7578) @@ -266,7 +266,7 @@ glPushMatrix(); - Gl::pose_inverse_shift( mod->GetPose() ); + Gl::pose_inverse_shift( mod->GetGlobalPose() ); glTranslatef( -width/2.0, -height/2.0, 0.01 ); glScalef( cellsize, cellsize, 1 ); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/libstage/stage.cc 2009-04-07 01:35:45 UTC (rev 7578) @@ -351,7 +351,7 @@ // inverting the original image. stg_rotrect_t *latest = &(*rects)[(*rect_count)-1]; latest->pose.x = startx; - latest->pose.y = img_height - (starty + height); + latest->pose.y = img_height-1 - (starty + height); latest->pose.a = 0.0; latest->size.x = x - startx; latest->size.y = height; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-06 00:58:00 UTC (rev 7577) +++ code/stage/trunk/libstage/stage.hh 2009-04-07 01:35:45 UTC (rev 7578) @@ -1631,7 +1631,23 @@ static GHashTable* modelsbyid; std::vector<Option*> drawOptions; const std::vector<Option*>& getOptions() const { return drawOptions; } - + + class RasterVis : public Visualizer + { + private: + uint8_t* data; + unsigned int width, height; + + public: + RasterVis(); + virtual ~RasterVis( void ){} + virtual void Visualize( Model* mod, Camera* cam ); + + void SetData( uint8_t* data, + unsigned int width, + unsigned int height ); + }; + protected: GMutex* access_mutex; GPtrArray* blinkenlights; @@ -1699,6 +1715,10 @@ by derived model types to store properties, and for user code to associate arbitrary items with a model. */ GData* props; + + /** Visualize the most recent rasterization operation performed by this model */ + RasterVis rastervis; + bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw char* say_string; ///< if non-null, this string is displayed in the GUI Modified: code/stage/trunk/worlds/bitmaps/SFU_1920x1280.png =================================================================== (Binary files differ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-09 22:51:15
|
Revision: 7591 http://playerstage.svn.sourceforge.net/playerstage/?rev=7591&view=rev Author: rtv Date: 2009-04-09 22:51:12 +0000 (Thu, 09 Apr 2009) Log Message: ----------- bugfix Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-09 22:51:12 UTC (rev 7591) @@ -504,7 +504,7 @@ { if( strcmp( mod->Token(), "r0" ) == 0 ) { - const unsigned int dw = 61, dh = 31; + const unsigned int dw = 64, dh = 32; uint8_t* data = new uint8_t[dw*dh*2]; memset( data, 0, sizeof(uint8_t) * dw * dh ); Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-04-09 22:51:12 UTC (rev 7591) @@ -4,7 +4,6 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs - vis_strip.cc ancestor.cc block.cc blockgroup.cc @@ -35,6 +34,7 @@ stage.hh texture_manager.cc typetable.cc + vis_strip.cc waypoint.cc world.cc worldfile.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/libstage/canvas.cc 2009-04-09 22:51:12 UTC (rev 7591) @@ -574,13 +574,13 @@ char str[64]; PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray - for( double i = floor(bounds.x.min); i < bounds.x.max; i++) + for( double i = ceil(bounds.x.min); i < bounds.x.max; i++) { snprintf( str, 16, "%d", (int)i ); Gl::draw_string( i, 0, 0.00, str ); } - for( double i = floor(bounds.y.min); i < bounds.y.max; i++) + for( double i = ceil(bounds.y.min); i < bounds.y.max; i++) { snprintf( str, 16, "%d", (int)i ); Gl::draw_string( 0, i, 0.00, str ); @@ -610,7 +610,25 @@ glDisable(GL_TEXTURE_2D); glEnable(GL_BLEND); - + +// glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); +// glTranslatef( 0,0,0.1 ); +// glColor3f( 0, 1.0, 0 ); + +// glBegin( GL_LINES ); +// for( float x=bounds.x.min; x<=bounds.x.max; x++ ) +// { +// glVertex2f( ceil(x), bounds.y.min ); +// glVertex2f( ceil(x), bounds.y.max ); +// } + +// for( float y=bounds.y.min; y<=bounds.y.max; y++ ) +// { +// glVertex2f( bounds.x.min, ceil(y) ); +// glVertex2f( bounds.x.max, ceil(y) ); +// } +// glEnd(); + glDisable(GL_POLYGON_OFFSET_FILL ); } Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/libstage/main.cc 2009-04-09 22:51:12 UTC (rev 7591) @@ -14,12 +14,12 @@ "USAGE: stage [options] [<worldfile>]\n" "Available [options] are:\n" " --clock : print simulation time peridically on standard output\n" - " -c : print simulation time peridically on standard output\n" + " -c : short for --clock\n" " --gui : run without a GUI\n" - " -g : run without a GUI\n" - " --help : print this message.\n" - " -h : print this message.\n" - " -? : print this message.\n" + " -g : short for --gui\n" + " --help : print this message\n" + " -h : short for --help\n" + " -? : short for --help\n" " If <worldfile> is not specified, Stage starts with a file selector dialog"; /* options descriptor */ Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/libstage/model_position.cc 2009-04-09 22:51:12 UTC (rev 7591) @@ -19,7 +19,6 @@ //#define DEBUG #include "stage.hh" -#include "option.hh" #include "worldfile.hh" using namespace Stg; @@ -86,14 +85,13 @@ const stg_position_localization_mode_t POSITION_LOCALIZATION_DEFAULT = STG_POSITION_LOCALIZATION_GPS; const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT = STG_POSITION_DRIVE_DIFFERENTIAL; -Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "", false, NULL ); -Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", true, NULL ); - ModelPosition::ModelPosition( World* world, Model* parent ) : Model( world, parent, MODEL_TYPE_POSITION ), waypoints( NULL ), - waypoint_count( 0 ) + waypoint_count( 0 ), + wpvis(), + posevis() { PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n", id, typestr ); @@ -132,8 +130,8 @@ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A - STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0; - RegisterOption( &showCoords ); - RegisterOption( &showWaypoints ); + AddVisualizer( &wpvis, true ); + AddVisualizer( &posevis, false ); } @@ -238,10 +236,6 @@ PRINT_ERR1( "no localization mode string specified for model \"%s\"", this->token ); } - - // TODO - // we've probably poked the localization data, so we must let people know - //model_change( mod, &mod->data ); } void ModelPosition::Update( void ) @@ -252,7 +246,7 @@ Velocity vel; memset( &vel, 0, sizeof(Velocity) ); - if( this->subs ) // no driving if noone is subscribed + if( this->subs ) // no driving if noone is subscribed { switch( control_mode ) { @@ -265,7 +259,6 @@ this->goal.y, this->goal.a ); - switch( drive_mode ) { case STG_POSITION_DRIVE_DIFFERENTIAL: @@ -334,8 +327,6 @@ // start out with no velocity Velocity calc; - memset( &calc, 0, sizeof(calc)); - double close_enough = 0.02; // fudge factor // if we're at the right spot @@ -388,16 +379,16 @@ // simple model of power consumption watts = STG_POSITION_WATTS + - fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + - fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + - fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; + fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", // this->token, // this->velocity.x, // this->velocity.y, // this->velocity.a ); - + this->SetVelocity( vel ); } @@ -421,21 +412,21 @@ case STG_POSITION_LOCALIZATION_ODOM: { - // integrate our velocities to get an 'odometry' position estimate. - double dt = this->world->GetSimInterval()/1e6; - - est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) ); - - double cosa = cos(est_pose.a); - double sina = sin(est_pose.a); - double dx = (vel.x * dt) * (1.0 + integration_error.x ); - double dy = (vel.y * dt) * (1.0 + integration_error.y ); - - est_pose.x += dx * cosa + dy * sina; - est_pose.y -= dy * cosa - dx * sina; + // integrate our velocities to get an 'odometry' position estimate. + double dt = this->world->GetSimInterval()/1e6; + + est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) ); + + double cosa = cos(est_pose.a); + double sina = sin(est_pose.a); + double dx = (vel.x * dt) * (1.0 + integration_error.x ); + double dy = (vel.y * dt) * (1.0 + integration_error.y ); + + est_pose.x += dx * cosa + dy * sina; + est_pose.y -= dy * cosa - dx * sina; } break; - + default: PRINT_ERR2( "unknown localization mode %d for model %s\n", localization_mode, this->token ); @@ -582,98 +573,108 @@ return replaced; } -void ModelPosition::DataVisualize( Camera* cam ) +ModelPosition::PoseVis::PoseVis() + : Visualizer( "Position coordinates", "show_position_coords" ) +{} + +void ModelPosition::PoseVis::Visualize( Model* mod, Camera* cam ) { - if( showWaypoints && waypoints && waypoint_count ) - DrawWaypoints(); + ModelPosition* pos = dynamic_cast<ModelPosition*>(mod); - if( showCoords ) - { - // vizualize my estimated pose - glPushMatrix(); + // vizualize my estimated pose + glPushMatrix(); - // back into global coords - Gl::pose_inverse_shift( GetGlobalPose() ); - - Gl::pose_shift( est_origin ); - PushColor( 1,0,0,1 ); // origin in red - Gl::draw_origin( 0.5 ); - - glEnable (GL_LINE_STIPPLE); - glLineStipple (3, 0xAAAA); - - PushColor( 1,0,0,0.5 ); - glBegin( GL_LINE_STRIP ); - glVertex2f( 0,0 ); - glVertex2f( est_pose.x, 0 ); - glVertex2f( est_pose.x, est_pose.y ); - glEnd(); + // back into global coords + Gl::pose_inverse_shift( pos->GetGlobalPose() ); + + Gl::pose_shift( pos->est_origin ); + pos->PushColor( 1,0,0,1 ); // origin in red + Gl::draw_origin( 0.5 ); - glDisable(GL_LINE_STIPPLE); + glEnable (GL_LINE_STIPPLE); + glLineStipple (3, 0xAAAA); + + pos->PushColor( 1,0,0,0.5 ); + glBegin( GL_LINE_STRIP ); + glVertex2f( 0,0 ); + glVertex2f( pos->est_pose.x, 0 ); + glVertex2f( pos->est_pose.x, pos->est_pose.y ); + glEnd(); + + glDisable(GL_LINE_STIPPLE); + + char label[64]; + snprintf( label, 64, "x:%.3f", pos->est_pose.x ); + Gl::draw_string( pos->est_pose.x / 2.0, -0.5, 0, label ); + + snprintf( label, 64, "y:%.3f", pos->est_pose.y ); + Gl::draw_string( pos->est_pose.x + 0.5 , pos->est_pose.y / 2.0, 0, (const char*)label ); + + pos->PopColor(); + + Gl::pose_shift( pos->est_pose ); + pos->PushColor( 0,1,0,1 ); // pose in green + Gl::draw_origin( 0.5 ); + pos->PopColor(); + + Gl::pose_shift( pos->geom.pose ); + pos->PushColor( 0,0,1,1 ); // offset in blue + Gl::draw_origin( 0.5 ); + pos->PopColor(); + + double r,g,b,a; + stg_color_unpack( pos->color, &r, &g, &b, &a ); + pos->PushColor( r, g, b, 0.5 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + pos->blockgroup.DrawFootPrint( pos->geom ); + pos->PopColor(); + + glPopMatrix(); +} - char label[64]; - snprintf( label, 64, "x:%.3f", est_pose.x ); - Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label ); - snprintf( label, 64, "y:%.3f", est_pose.y ); - Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const char*)label ); - - PopColor(); +ModelPosition::WaypointVis::WaypointVis() + : Visualizer( "Position waypoints", "show_position_waypoints" ) +{} - Gl::pose_shift( est_pose ); - PushColor( 0,1,0,1 ); // pose in green - Gl::draw_origin( 0.5 ); - PopColor(); +void ModelPosition::WaypointVis::Visualize( Model* mod, Camera* cam ) +{ + ModelPosition* pos = dynamic_cast<ModelPosition*>(mod); - Gl::pose_shift( geom.pose ); - PushColor( 0,0,1,1 ); // offset in blue - Gl::draw_origin( 0.5 ); - PopColor(); - - double r,g,b,a; - stg_color_unpack( color, &r, &g, &b, &a ); - PushColor( r, g, b, 0.5 ); - - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - blockgroup.DrawFootPrint( geom ); - PopColor(); + Waypoint* waypoints = pos->waypoints; + unsigned int waypoint_count = pos->waypoint_count; - glPopMatrix(); - } + if( (waypoints == NULL) || (waypoint_count < 1) ) + return; - // inherit more viz - Model::DataVisualize( cam ); -} - -void ModelPosition::DrawWaypoints() -{ glPointSize( 5 ); glPushMatrix(); - PushColor( color ); + pos->PushColor( pos->color ); - Gl::pose_inverse_shift( pose ); - Gl::pose_shift( est_origin ); + Gl::pose_inverse_shift( pos->pose ); + Gl::pose_shift( pos->est_origin ); glTranslatef( 0,0,0.02 ); - + // draw waypoints for( unsigned int i=0; i < waypoint_count; i++ ) - waypoints[i].Draw(); - + waypoints[i].Draw(); + // draw lines connecting the waypoints if( waypoint_count > 1 ) { glBegin( GL_LINES ); for( unsigned int i=1; i < waypoint_count; i++ ) - { - glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y ); - glVertex2f( waypoints[i-1].pose.x, waypoints[i-1].pose.y ); - } - + { + glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y ); + glVertex2f( waypoints[i-1].pose.x, waypoints[i-1].pose.y ); + } + glEnd(); } - PopColor(); + pos->PopColor(); glPopMatrix(); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-09 18:16:27 UTC (rev 7590) +++ code/stage/trunk/libstage/stage.hh 2009-04-09 22:51:12 UTC (rev 7591) @@ -373,7 +373,6 @@ Bounds z; } stg_bounds3d_t; - /** Define a three-dimensional bounding box, initialized to zero */ typedef struct { @@ -568,7 +567,6 @@ // MACROS ------------------------------------------------------ // Some useful macros - /** Look up the color in the X11 database. (i.e. transform color name to color value). If the color is not found in the database, a bright red color (0xF00) will be returned instead. @@ -2768,12 +2766,7 @@ Waypoint* waypoints; uint32_t waypoint_count; - void DrawWaypoints(); - private: - static Option showCoords; - static Option showWaypoints; - public: static const char* typestr; // constructor @@ -2787,11 +2780,29 @@ virtual void Update(); virtual void Load(); - virtual void DataVisualize( Camera* cam ); - /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); + class WaypointVis : public Visualizer + { + public: + WaypointVis(); + virtual ~WaypointVis( void ){} + virtual void Visualize( Model* mod, Camera* cam ); + }; + + WaypointVis wpvis; + + class PoseVis : public Visualizer + { + public: + PoseVis(); + virtual ~PoseVis( void ){} + virtual void Visualize( Model* mod, Camera* cam ); + }; + + PoseVis posevis; + /** Set the current pose estimate.*/ void SetOdom( Pose odom ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-11 00:39:46
|
Revision: 7594 http://playerstage.svn.sourceforge.net/playerstage/?rev=7594&view=rev Author: rtv Date: 2009-04-11 00:39:13 +0000 (Sat, 11 Apr 2009) Log Message: ----------- fixed raserizer scaling bug Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -505,10 +505,11 @@ if( strcmp( mod->Token(), "r0" ) == 0 ) { const unsigned int dw = 64, dh = 32; + uint8_t* data = new uint8_t[dw*dh*2]; memset( data, 0, sizeof(uint8_t) * dw * dh ); - mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh ); + mod->GetWorld()->GetModel( "cave" )->Rasterize( data, dw, dh, 0.25, 0.5 ); putchar( '\n' ); for( unsigned int y=0; y<dh; y++ ) Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/block.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -50,7 +50,6 @@ Load( wf, entity ); } - Block::~Block() { if( mapped ) UnMap(); @@ -72,7 +71,6 @@ mod->blockgroup.BuildDisplayList( mod ); } - double Block::CenterY() { double min = billion; @@ -254,6 +252,25 @@ mapped = true; } +stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt ) +{ + Pose gpose = mod->GetGlobalPose(); + gpose = pose_sum( gpose, mod->geom.pose ); // add local offset + + Size bgsize = mod->blockgroup.GetSize(); + stg_point3_t bgoffset = mod->blockgroup.GetOffset(); + + stg_point3_t scale; + scale.x = mod->geom.size.x / bgsize.x; + scale.y = mod->geom.size.y / bgsize.y; + scale.z = mod->geom.size.z / bgsize.z; + + stg_point_t mpt; + mpt.x = (bpt.x - bgoffset.x) * scale.x; + mpt.y = (bpt.y - bgoffset.y) * scale.y; + return mpt; +} + void Block::GenerateCandidateCells() { @@ -313,131 +330,168 @@ mapped = true; } -void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) -{ - // add local offset - // pose = pose_sum( pose, mod->geom.pose ); +// void Block::Rasterize( uint8_t* data, +// unsigned int width, +// unsigned int height, +// stg_meters_t cellwidth, +// stg_meters_t cellheight ) +// { +// // add local offset +// // pose = pose_sum( pose, mod->geom.pose ); - Size bgsize = mod->blockgroup.GetSize(); - - double scalex = (double)(width) / (double)bgsize.x; - double scaley = (double)(height) / (double)bgsize.y; - //double scalex = (width) / bgsize.x; - //double scaley = (height) / bgsize.y; +// Size bgsize = mod->blockgroup.GetSize(); + +// double scalex = (width*cellwidth) / bgsize.x; +// double scaley = (height*cellheight) / bgsize.y; - Rasterize( data, width, height, scalex, scaley, 0,0 ); -} - -// void swap( int& a, int& b ) -// { -// int tmp = a; -// a = b; -// b = tmp; +// Rasterize( data, width, height, scalex, scaley, 0,0 ); // } -void swap( int* a, int* b ) +void swap( int& a, int& b ) { - int foo = *a; - *a = *b; - *b = foo; + int tmp = a; + a = b; + b = tmp; } -void Block::Rasterize( uint8_t* data, - unsigned int width, unsigned int height, - double scalex, double scaley, - double offsetx, double offsety ) -{ - //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", - // this, width, height, scalex, scaley, offsetx, offsety ); +// void Block::Rasterize( uint8_t* data, +// unsigned int width, unsigned int height, +// double scalex, double scaley, +// double offsetx, double offsety ) +// { +// //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", +// // this, width, height, scalex, scaley, offsetx, offsety ); - unsigned int W=0; +// for( unsigned int i=0; i<pt_count; i++ ) +// { +// double px = pts[i].x; +// double py = pts[i].y; -// W+=20; -// W /= 2; - -// printf( "W is %u", W ); +// //unsigned int keep_i = i; - for( W=0; W<pt_count; W++ ) - { - double px = pts[W].x; - double py = pts[W].y; +// int xa = floor( (pts[i ].x + offsetx) * scalex ); +// int ya = floor( (pts[i ].y + offsety) * scaley ); +// int xb = floor( (pts[(i+1)%pt_count].x + offsetx) * scalex ); +// int yb = floor( (pts[(i+1)%pt_count].y + offsety) * scaley ); - //unsigned int keep_W = W; +// mod->rastervis.AddPoint( px, py ); - int xa = floor( (pts[W ].x + offsetx) * scalex ); - int ya = floor( (pts[W ].y + offsety) * scaley ); - int xb = floor( (pts[(W+1)%pt_count].x + offsetx) * scalex ); - int yb = floor( (pts[(W+1)%pt_count].y + offsety) * scaley ); +// //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); + +// bool steep = abs( yb-ya ) > abs( xb-xa ); +// if( steep ) +// { +// swap( xa, ya ); +// swap( xb, yb ); +// } + +// if( xa > xb ) +// { +// swap( xa, xb ); +// swap( ya, yb ); +// } + +// double dydx = (double) (yb - ya) / (double) (xb - xa); +// double y = ya; +// for(int x=xa; x<=xb; x++) +// { +// if( steep ) +// { +// if( ! (floor(y) >= 0) ) continue; +// if( ! (floor(y) < (int)width) ) continue; +// if( ! (x >= 0) ) continue; +// if( ! (x < (int)height) ) continue; +// } +// else +// { +// if( ! (x >= 0) ) continue; +// if( ! (x < (int)width) ) continue; +// if( ! (floor(y) >= 0) ) continue; +// if( ! (floor(y) < (int)height) ) continue; +// } + +// if( steep ) +// data[ (int)floor(y) + (x * width)] = 1; +// else +// data[ x + ((int)floor(y) * width)] = 1; +// y += dydx; +// } +// } +// } - mod->rastervis.AddPoint( px, py ); +void Block::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) +{ + //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", + // this, width, height, scalex, scaley, offsetx, offsety ); - //int keep_xa = xa; - //int keep_xb = xb; + for( unsigned int i=0; i<pt_count; i++ ) + { + // convert points from local to model coords + stg_point_t mpt1 = BlockPointToModelMeters( pts[i] ); + stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] ); + // record for debug visualization + mod->rastervis.AddPoint( mpt1.x, mpt1.y ); + + // shift to the bottom left of the model + mpt1.x += mod->geom.size.x/2.0; + mpt1.y += mod->geom.size.y/2.0; + mpt2.x += mod->geom.size.x/2.0; + mpt2.y += mod->geom.size.y/2.0; + + // convert from meters to cells + int xa = floor( mpt1.x / cellwidth ); + int ya = floor( mpt1.y / cellheight ); + int xb = floor( mpt2.x / cellwidth ); + int yb = floor( mpt2.y / cellheight ); //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); bool steep = abs( yb-ya ) > abs( xb-xa ); if( steep ) { - swap( &xa, &ya ); - swap( &xb, &yb ); + swap( xa, ya ); + swap( xb, yb ); } if( xa > xb ) { - swap( &xa, &xb ); - swap( &ya, &yb ); + swap( xa, xb ); + swap( ya, yb ); } double dydx = (double) (yb - ya) / (double) (xb - xa); double y = ya; for(int x=xa; x<=xb; x++) { - // if( steep ) -// { -// if( ! (floor(y) >= 0) ) continue; -// if( ! (floor(y) < (int)width) ) continue; -// if( ! (x >= 0) ) continue; -// if( ! (x < (int)height) ) continue; -// } -// else -// { -// if( ! (x >= 0) ) continue; -// if( ! (x < (int)width) ) continue; -// if( ! (floor(y) >= 0) ) continue; -// if( ! (floor(y) < (int)height) ) continue; -// } + if( steep ) + { + if( ! (floor(y) >= 0) ) continue; + if( ! (floor(y) < (int)width) ) continue; + if( ! (x >= 0) ) continue; + if( ! (x < (int)height) ) continue; + } + else + { + if( ! (x >= 0) ) continue; + if( ! (x < (int)width) ) continue; + if( ! (floor(y) >= 0) ) continue; + if( ! (floor(y) < (int)height) ) continue; + } if( steep ) data[ (int)floor(y) + (x * width)] = 1; else data[ x + ((int)floor(y) * width)] = 1; y += dydx; - -// if( (floor(y) == 75) && -// x == 119 ) -// { -// puts( "foo" ); -// // while(1) {} - -// printf( "W: %u keep_W: %u px: %.4f\npy: %.4f\n", -// W, keep_W, -// px, py ); - -// printf( "XA: %.4f\nXB: %.4f\n", -// (pts[W ].x + offsetx) * scalex, -// (pts[(W+1)%pt_count].x + offsetx) * scalex ); - -// printf( "KEEP: %d %d\n", keep_xa, keep_xb ); -// printf( "NOW: %d %d\n", xa, xb ); -// } - } } } - void Block::DrawTop() { // draw the top of the block - a polygon at the highest vertical Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -310,8 +310,12 @@ } -void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +void BlockGroup::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { for( GList* it = blocks; it; it=it->next ) - ((Block*)it->data)->Rasterize( data, width, height ); + ((Block*)it->data)->Rasterize( data, width, height, cellwidth, cellheight ); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/model.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -1025,11 +1025,15 @@ } -void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) +void Model::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { rastervis.ClearPts(); - blockgroup.Rasterize( data, width, height ); - rastervis.SetData( data, width, height ); + blockgroup.Rasterize( data, width, height, cellwidth, cellheight ); + rastervis.SetData( data, width, height, cellwidth, cellheight ); } //*************************************************************** @@ -1040,6 +1044,8 @@ data(NULL), width(0), height(0), + cellwidth(0), + cellheight(0), pts(NULL) { } @@ -1060,8 +1066,8 @@ { glPushMatrix(); Size sz = mod->blockgroup.GetSize(); - glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); - glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); + //glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); + //glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); // now we're in world meters coordinates glPointSize( 4 ); @@ -1085,8 +1091,10 @@ // go into bitmap pixel coords glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); - glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + //glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + glScalef( cellwidth, cellheight, 1 ); + mod->PushColor( 0,0,0,0.5 ); glPolygonMode( GL_FRONT, GL_FILL ); for( unsigned int y=0; y<height; y++ ) @@ -1131,7 +1139,9 @@ void Model::RasterVis::SetData( uint8_t* data, unsigned int width, - unsigned int height ) + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { // copy the raster for test visualization if( this->data ) @@ -1142,6 +1152,8 @@ memcpy( this->data, data, len ); this->width = width; this->height = height; + this->cellwidth = cellwidth; + this->cellheight = cellheight; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/stage.hh 2009-04-11 00:39:13 UTC (rev 7594) @@ -1128,14 +1128,15 @@ Model* GetModel(){ return mod; }; stg_color_t GetColor(); - - void Rasterize( uint8_t* data, - unsigned int width, unsigned int height, - double scalex, double scaley, - double offsetx, double offsety ); +// void Rasterize( uint8_t* data, +// unsigned int width, unsigned int height, +// double scalex, double scaley, +// double offsetx, double offsety ); + void Rasterize( uint8_t* data, - unsigned int width, unsigned int height ); + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); private: Model* mod; ///< model to which this block belongs @@ -1169,6 +1170,11 @@ written, and the pointers to the rendered and potential cells are switched for next time (avoiding a memory copy).*/ GPtrArray* candidate_cells; + + + // find the position of a block's internal point in meters + // relative to the model + stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); }; @@ -1222,15 +1228,16 @@ void LoadBitmap( Model* mod, const char* bitmapfile, Worldfile *wf ); void LoadBlock( Model* mod, Worldfile* wf, int entity ); - - void Rasterize( uint8_t* data, unsigned int width, unsigned int height ); + + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); }; typedef int ctrlinit_t( Model* mod ); //typedef void ctrlupdate_t( Model* mod ); - - + // BLOCKS class Camera @@ -1642,6 +1649,7 @@ private: uint8_t* data; unsigned int width, height; + stg_meters_t cellwidth, cellheight; GList* pts; public: @@ -1651,7 +1659,9 @@ void SetData( uint8_t* data, unsigned int width, - unsigned int height ); + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ); void AddPoint( stg_meters_t x, stg_meters_t y ); void ClearPts(); @@ -1764,7 +1774,9 @@ /** Render the model's blocks as an occupancy grid into the preallocated array of width by height pixels */ - void Rasterize( uint8_t* data, unsigned int width, unsigned int height ); + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); void Lock() { Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/libstage/worldgui.cc 2009-04-11 00:39:13 UTC (rev 7594) @@ -388,7 +388,7 @@ char buf[32]; snprintf( buf, 32, " [%.2f]", localratio ); - str + buf; + str += buf; if( paused == true ) str += " [ PAUSED ]"; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-04-10 14:58:25 UTC (rev 7593) +++ code/stage/trunk/worlds/fasr.world 2009-04-11 00:39:13 UTC (rev 7594) @@ -40,8 +40,11 @@ floorplan ( name "cave" - size [16.000 16.000 0.600] pose [0 0 0 0] + #size [1000.000 750.000 0.600] + #bitmap "bitmaps/SFU_medium_1.png" + + size [16 16 0.600] bitmap "bitmaps/cave.png" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-14 18:53:39
|
Revision: 7598 http://playerstage.svn.sourceforge.net/playerstage/?rev=7598&view=rev Author: rtv Date: 2009-04-14 18:53:06 +0000 (Tue, 14 Apr 2009) Log Message: ----------- patched blobfinder, thanks to wliu Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-13 08:39:21 UTC (rev 7597) +++ code/stage/trunk/libstage/model.cc 2009-04-14 18:53:06 UTC (rev 7598) @@ -717,7 +717,8 @@ void Model::CallUpdateCallbacks( void ) { - if( last_update == world->sim_time ) + // if we were updated this timestep, call the callbacks + if( last_update == world->sim_time ) CallCallbacks( &hooks.update ); } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-13 08:39:21 UTC (rev 7597) +++ code/stage/trunk/libstage/world.cc 2009-04-14 18:53:06 UTC (rev 7598) @@ -573,7 +573,7 @@ { // find the direction of the first ray Pose raypose = pose; - double starta = fov/2.0; + double starta = fov/2.0 - raypose.a; for( uint32_t s=0; s < sample_count; s++ ) { Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-04-13 08:39:21 UTC (rev 7597) +++ code/stage/trunk/worlds/fasr.world 2009-04-14 18:53:06 UTC (rev 7598) @@ -41,9 +41,6 @@ ( name "cave" pose [0 0 0 0] - #size [1000.000 750.000 0.600] - #bitmap "bitmaps/SFU_medium_1.png" - size [16 16 0.600] bitmap "bitmaps/cave.png" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-05 00:13:51
|
Revision: 7618 http://playerstage.svn.sourceforge.net/playerstage/?rev=7618&view=rev Author: rtv Date: 2009-05-05 00:12:13 +0000 (Tue, 05 May 2009) Log Message: ----------- working on core raytracing code Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-05-04 20:49:10 UTC (rev 7617) +++ code/stage/trunk/libstage/model.cc 2009-05-05 00:12:13 UTC (rev 7618) @@ -557,16 +557,16 @@ return candidate->IsDescendent( that ); } -inline Pose Model::LocalToGlobal( const Pose& pose ) const -{ - return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); -} +// Pose Model::LocalToGlobal( const Pose& pose ) const +// { +// return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); +// } stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const -{ - Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) ); - return stg_point_t( gpose.x, gpose.y ); -} + { + Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) ); + return stg_point_t( gpose.x, gpose.y ); + } void Model::MapWithChildren() { Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-05-04 20:49:10 UTC (rev 7617) +++ code/stage/trunk/libstage/model_laser.cc 2009-05-05 00:12:13 UTC (rev 7618) @@ -1,6 +1,4 @@ -/////////////////////////////////////////////////////////////////////////// -// -// File: model_laser.c +// file: model_laser.c // Author: Richard Vaughan // Date: 10 June 2004 // @@ -91,10 +89,7 @@ ModelLaser* laser = dynamic_cast<ModelLaser*>(mod); unsigned int sample_count = 0; stg_laser_sample_t* samples = laser->GetSamples( &sample_count ); - - if( ! (samples && sample_count) ) - return; - + glPushMatrix(); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); @@ -193,11 +188,12 @@ vis( world ), data_dl(0), data_dirty( true ), - samples( NULL ), // don't allocate sample buffer memory until Update() is called sample_count( DEFAULT_SAMPLES ), + samples(), range_max( DEFAULT_MAXRANGE ), fov( DEFAULT_FOV ), - resolution( DEFAULT_RESOLUTION ) + resolution( DEFAULT_RESOLUTION ), + rays() { PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n", @@ -212,6 +208,9 @@ geom.size = DEFAULT_SIZE; SetGeom( geom ); + rays.resize( sample_count ); + samples.resize( sample_count ); + // assert that Update() is reentrant for this derived model thread_safe = true; @@ -224,11 +223,6 @@ ModelLaser::~ModelLaser( void ) { - if(samples) - { - g_free( samples ); - samples = NULL; - } } void ModelLaser::Load( void ) @@ -248,6 +242,9 @@ } Model::Load(); + + rays.resize(sample_count); + samples.resize(sample_count); } stg_laser_cfg_t ModelLaser::GetConfig() @@ -267,6 +264,10 @@ fov = cfg.fov; resolution = cfg.resolution; interval = cfg.interval; + sample_count = cfg.sample_count; + + samples.resize( sample_count ); + rays.resize( sample_count ); } static bool laser_raytrace_match( Model* hit, @@ -278,62 +279,82 @@ return( (hit != finder) && (hit->vis.laser_return > 0 ) ); } +// void ModelLaser::SetSampleCount( unsigned int count ) +// { +// sample_count = count; + +// samples.resize( count ); +// rays.resize( count ); + void ModelLaser::Update( void ) { double bearing = -fov/2.0; double sample_incr = fov / (double)(sample_count-1); - samples = g_renew( stg_laser_sample_t, samples, sample_count ); - Pose rayorg = geom.pose; bzero( &rayorg, sizeof(rayorg)); rayorg.z += geom.size.z/2; + assert( samples.size() == sample_count ); + for( unsigned int t=0; t<sample_count; t += resolution ) { rayorg.a = bearing; + + // set up the ray + rays[t].origin = LocalToGlobal(rayorg); - stg_raytrace_result_t sample = - Raytrace( rayorg, - range_max, - laser_raytrace_match, - NULL, - true ); // z testing enabled + // todo - do this constant stuff in advance + rays[t].func = laser_raytrace_match; + rays[t].arg = NULL; + rays[t].range = range_max; + rays[t].arg = NULL; + rays[t].ztest = true; + rays[t].mod = this; - samples[t].range = sample.range; + bearing += sample_incr; + } + + // do the raytracing of all rays in one go (allows parallel implementation) + world->Raytrace( rays ); + + // now process the results and fill in our samples + for( unsigned int t=0; t<sample_count; t += resolution ) + { + stg_raytrace_result_t* r = &rays[t].result; + assert( r ); + samples[t].range = r->range; + // if we hit a model and it reflects brightly, we set // reflectance high, else low - if( sample.mod && ( sample.mod->vis.laser_return >= LaserBright ) ) - samples[t].reflectance = 1; + if( r->mod && ( r->mod->vis.laser_return >= LaserBright ) ) + samples[t].reflectance = 1; else - samples[t].reflectance = 0; - - // todo - lower bound on range - bearing += sample_incr; + samples[t].reflectance = 0; } - + // we may need to interpolate the samples we skipped if( resolution > 1 ) { for( unsigned int t=resolution; t<sample_count; t+=resolution ) - for( unsigned int g=1; g<resolution; g++ ) - { - if( t >= sample_count ) - break; + for( unsigned int g=1; g<resolution; g++ ) + { + if( t >= sample_count ) + break; - // copy the rightmost sample data into this point - memcpy( &samples[t-g], - &samples[t-resolution], - sizeof(stg_laser_sample_t)); + // copy the rightmost sample data into this point + memcpy( &samples[t-g], + &samples[t-resolution], + sizeof(stg_laser_sample_t)); - double left = samples[t].range; - double right = samples[t-resolution].range; + double left = samples[t].range; + double right = samples[t-resolution].range; - // linear range interpolation between the left and right samples - samples[t-g].range = (left-g*(left-right)/resolution); - } - } + // linear range interpolation between the left and right samples + samples[t-g].range = (left-g*(left-right)/resolution); + } + } data_dirty = true; @@ -358,13 +379,7 @@ // stop consuming power SetWatts( 0 ); - // clear the data - if(samples) - { - g_free( samples ); - samples = NULL; - } - + Model::Shutdown(); } @@ -373,22 +388,13 @@ Model::Print( prefix ); printf( "\tRanges[ " ); - - if( samples ) - for( unsigned int i=0; i<sample_count; i++ ) + for( unsigned int i=0; i<sample_count; i++ ) printf( "%.2f ", samples[i].range ); - else - printf( "<none until subscribed>" ); puts( " ]" ); printf( "\tReflectance[ " ); - - if( samples ) - for( unsigned int i=0; i<sample_count; i++ ) - printf( "%.2f ", samples[i].reflectance ); - else - printf( "<none until subscribed>" ); - + for( unsigned int i=0; i<sample_count; i++ ) + printf( "%.2f ", samples[i].reflectance ); puts( " ]" ); } @@ -396,17 +402,5 @@ stg_laser_sample_t* ModelLaser::GetSamples( uint32_t* count ) { if( count ) *count = sample_count; - return samples; + return &samples[0]; } - -void ModelLaser::SetSamples( stg_laser_sample_t* samples, uint32_t count) -{ - this->samples = g_renew( stg_laser_sample_t, this->samples, sample_count ); - memcpy( this->samples, samples, sample_count * sizeof(stg_laser_sample_t)); - this->sample_count = count; - this->data_dirty = true; -} - - - - Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-04 20:49:10 UTC (rev 7617) +++ code/stage/trunk/libstage/stage.hh 2009-05-05 00:12:13 UTC (rev 7618) @@ -825,6 +825,27 @@ } stg_raytrace_result_t; + class Ray + { + public: + //SuperRegion& sup; + //Regiion& reg; + //Cell& cell; + //stg_point_int_t glob; + //stg_point_int_t origin; + //stg_point_int_t dest; + + Model* mod; + Pose origin; + stg_meters_t range; + stg_ray_test_func_t func; + void* arg; + bool ztest; + + stg_raytrace_result_t result; + }; + + const uint32_t INTERVAL_LOG_LEN = 32; // defined in stage_internal.hh @@ -975,6 +996,9 @@ SuperRegion* CreateSuperRegion( stg_point_int_t origin ); void DestroySuperRegion( SuperRegion* sr ); + // trace a vector of rays all in one go + void Raytrace( std::vector<Ray>& rays ); + stg_raytrace_result_t Raytrace( const Pose& pose, const stg_meters_t range, const stg_ray_test_func_t func, @@ -2298,7 +2322,10 @@ /** Return the global pose (i.e. pose in world coordinates) of a pose specified in the model's local coordinate system */ - Pose LocalToGlobal( const Pose& pose ) const; + Pose LocalToGlobal( const Pose& pose ) const + { + return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); + } // /** Return the 3d point in world coordinates of a 3d point // specified in the model's local coordinate system */ @@ -2420,24 +2447,31 @@ static Option showStrikes; static Option showFov; static Option showBeams; - + public: Vis( World* world ); virtual ~Vis( void ){} virtual void Visualize( Model* mod, Camera* cam ); }; - + Vis vis; + //class LaserRay : public Ray + /** OpenGL displaylist for laser data */ int data_dl; bool data_dirty; - stg_laser_sample_t* samples; + //stg_laser_sample_t* samples; uint32_t sample_count; + + std::vector<stg_laser_sample_t> samples; + stg_meters_t range_max; stg_radians_t fov; uint32_t resolution; + + std::vector<Ray> rays; public: static const char* typestr; @@ -2458,7 +2492,7 @@ stg_laser_sample_t* GetSamples( uint32_t* count=NULL); - void SetSamples( stg_laser_sample_t* samples, uint32_t count); + //void SetSamples( stg_laser_sample_t* samples, uint32_t count); // Get the user-tweakable configuration of the laser stg_laser_cfg_t GetConfig( ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-04 20:49:10 UTC (rev 7617) +++ code/stage/trunk/libstage/world.cc 2009-05-05 00:12:13 UTC (rev 7618) @@ -612,7 +612,18 @@ } -void World::Raytrace( const Pose &pose, // global pose +void World::Raytrace( std::vector<Ray>& rays ) +{ + for( std::vector<Ray>::iterator it = rays.begin(); + it != rays.end(); + ++it ) + { + Ray& r = *it; + r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg, r.ztest ); + } +} + +void World::Raytrace( const Pose &gpose, // global pose const stg_meters_t range, const stg_radians_t fov, const stg_ray_test_func_t func, @@ -623,7 +634,7 @@ const bool ztest ) { // find the direction of the first ray - Pose raypose = pose; + Pose raypose = gpose; double starta = fov/2.0 - raypose.a; for( uint32_t s=0; s < sample_count; s++ ) @@ -711,6 +722,10 @@ // and the x and y offsets of the ray int32_t dx = (int32_t)(ppm*range * cos(gpose.a)); int32_t dy = (int32_t)(ppm*range * sin(gpose.a)); + +// // the number of regions we will travel through +// int32_t rdx = dx / Region::WIDTH; +// int32_t rdy = dy / Region::WIDTH; // if( finder->debug ) // RecordRay( pose.x, @@ -721,6 +736,7 @@ // fast integer line 3d algorithm adapted from Cohen's code from // Graphics Gems IV + // cell unti int sx = sgn(dx); // sgn() is a fast macro int sy = sgn(dy); int ax = abs(dx); @@ -729,7 +745,17 @@ int by = 2*ay; int exy = ay-ax; int n = ax+ay; - + +// // region units +// int rsx = sgn(rdx); // sgn() is a fast macro +// int rsy = sgn(rdy); +// int rax = abs(rdx); +// int ray = abs(rdy); +// int rbx = 2*rax; +// int rby = 2*ray; +// int rexy = ray-rax; +// int rn = rax+ray; + // printf( "Raytracing from (%d,%d) steps (%d,%d) %d\n", // x,y, dx,dy, n ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-05-04 20:49:10 UTC (rev 7617) +++ code/stage/trunk/worlds/fasr.world 2009-05-05 00:12:13 UTC (rev 7618) @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 16 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 180 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" joules 100000 joules_capacity 400000 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-12 23:48:34
|
Revision: 7639 http://playerstage.svn.sourceforge.net/playerstage/?rev=7639&view=rev Author: rtv Date: 2009-05-12 23:48:06 +0000 (Tue, 12 May 2009) Log Message: ----------- reverted worldfile.cc changes that broke model property setting Modified Paths: -------------- code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-05-12 15:35:15 UTC (rev 7638) +++ code/stage/trunk/libstage/region.hh 2009-05-12 23:48:06 UTC (rev 7639) @@ -15,7 +15,7 @@ // a bit of experimenting suggests that these values are fast. YMMV. #define RBITS 4 // regions contain (2^RBITS)^2 pixels -#define SBITS 4 // superregions contain (2^SBITS)^2 regions +#define SBITS 6 // superregions contain (2^SBITS)^2 regions #define SRBITS (RBITS+SBITS) #define REGIONWIDTH (1<<RBITS) Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-12 15:35:15 UTC (rev 7638) +++ code/stage/trunk/libstage/world.cc 2009-05-12 23:48:06 UTC (rev 7639) @@ -1127,4 +1127,4 @@ printf( "log entry count %lu\n", LogEntry::Count() ); //LogEntry::Print(); } - + Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2009-05-12 15:35:15 UTC (rev 7638) +++ code/stage/trunk/libstage/worldfile.cc 2009-05-12 23:48:06 UTC (rev 7639) @@ -103,7 +103,9 @@ this->unit_length = 1.0; this->unit_angle = M_PI / 180; - this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); + // this attempt to fix memory leak breaks parsing of model properties - investigate + //this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); + this->nametable = g_hash_table_new( g_str_hash, g_str_equal ); } @@ -1345,7 +1347,10 @@ if( this->nametable ) g_hash_table_destroy( this->nametable ); - this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); + + // this attempt to fix memory leak breaks parsing of model properties - investigate + //this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); + this->nametable = g_hash_table_new( g_str_hash, g_str_equal ); } Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-05-12 15:35:15 UTC (rev 7638) +++ code/stage/trunk/worlds/fasr.world 2009-05-12 23:48:06 UTC (rev 7639) @@ -70,24 +70,6 @@ obstacle_return 0 ) -define autorob pioneer2dx -( - sicklaser( samples 180 range_max 5 laser_return 2 watts 30 ) - ctrl "fasr" - joules 100000 - joules_capacity 400000 - fiducial_return 0 - # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) - - gripper( pose [0.250 0 -0.220 0] - take_watts 1000.0 - fiducial( range 3 ) - # paddles [ "closed" "up" ] - obstacle_return 0 # cheating for simplicity - # autosnatch 1 - ) -) - define charge_station model ( size [ 0.100 0.300 0.100 ] @@ -153,6 +135,24 @@ #puck( pose [ 1.067 3.367 0 0 ] ) #puck( pose [ 1.412 3.604 0 0 ] ) +define autorob pioneer2dx +( + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + ctrl "fasr" + joules 100000 + joules_capacity 400000 + fiducial_return 0 + # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + + gripper( pose [0.250 0 -0.220 0] + take_watts 1000.0 + fiducial( range 3 ) + # paddles [ "closed" "up" ] + obstacle_return 0 # cheating for simplicity + # autosnatch 1 + ) +) + autorob( pose [5.488 5.149 0 35.947] joules 300000 name "r0" ) autorob( pose [6.431 5.593 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-13 17:57:41
|
Revision: 7645 http://playerstage.svn.sourceforge.net/playerstage/?rev=7645&view=rev Author: rtv Date: 2009-05-13 17:57:36 +0000 (Wed, 13 May 2009) Log Message: ----------- replaced hash table with stl::map in inner RT loop, increased superregion size Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-05-13 17:57:36 UTC (rev 7645) @@ -97,6 +97,6 @@ ) INSTALL(FILES stage.hh - DESTINATION include/${PROJECT_NAME}-${V_MAJOR}.${V_MINOR}) + DESTINATION include/${PROJECT_NAME}-${APIVERSION}) ADD_TEST( test1 ${EXECUTABLE_OUTPUT_PATH}stagetest ) Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/model_laser.cc 2009-05-13 17:57:36 UTC (rev 7645) @@ -181,7 +181,7 @@ glPopMatrix(); } - + ModelLaser::ModelLaser( World* world, Model* parent ) : Model( world, parent, MODEL_TYPE_LASER ), @@ -208,16 +208,16 @@ geom.size = DEFAULT_SIZE; SetGeom( geom ); - rays.resize( sample_count ); - samples.resize( sample_count ); - // assert that Update() is reentrant for this derived model thread_safe = true; // set default color SetColor( stg_lookup_color(DEFAULT_COLOR)); - AddVisualizer( &vis, true ); + // set up our data buffers and raytracing + SampleConfig(); + + AddVisualizer( &vis, true ); } @@ -242,9 +242,8 @@ } Model::Load(); - - rays.resize(sample_count); - samples.resize(sample_count); + + SampleConfig(); } stg_laser_cfg_t ModelLaser::GetConfig() @@ -266,8 +265,7 @@ interval = cfg.interval; sample_count = cfg.sample_count; - samples.resize( sample_count ); - rays.resize( sample_count ); + SampleConfig(); } static bool laser_raytrace_match( Model* hit, @@ -279,40 +277,40 @@ return( (hit != finder) && (hit->vis.laser_return > 0 ) ); } -// void ModelLaser::SetSampleCount( unsigned int count ) -// { -// sample_count = count; +void ModelLaser::SampleConfig() +{ + samples.resize( sample_count ); + rays.resize( sample_count ); -// samples.resize( count ); -// rays.resize( count ); + for( unsigned int t=0; t<sample_count; ++t ) + { + // configure a normal ray with our laser-specific settings + rays[t].func = laser_raytrace_match; + rays[t].arg = NULL; + rays[t].ztest = true; + rays[t].range = range_max; + rays[t].mod = this; + } +} void ModelLaser::Update( void ) { + assert( samples.size() == sample_count ); + assert( rays.size() == sample_count ); + double bearing = -fov/2.0; - double sample_incr = fov / (double)(sample_count-1); + double sample_incr = fov / (double)sample_count; Pose rayorg = geom.pose; - bzero( &rayorg, sizeof(rayorg)); - rayorg.z += geom.size.z/2; + rayorg.z += geom.size.z/2.0; + rayorg.a = bearing; + rayorg = LocalToGlobal(rayorg); - assert( samples.size() == sample_count ); - + // set up the ray origins in global coords for( unsigned int t=0; t<sample_count; t += resolution ) { - rayorg.a = bearing; - - // set up the ray - rays[t].origin = LocalToGlobal(rayorg); - - // todo - do this constant stuff in advance - rays[t].func = laser_raytrace_match; - rays[t].arg = NULL; - rays[t].range = range_max; - rays[t].arg = NULL; - rays[t].ztest = true; - rays[t].mod = this; - - bearing += sample_incr; + rays[t].origin = rayorg; + rayorg.a += sample_incr; } // do the raytracing of all rays in one go (allows parallel implementation) @@ -375,11 +373,7 @@ void ModelLaser::Shutdown( void ) { PRINT_DEBUG( "laser shutdown" ); - - // stop consuming power - SetWatts( 0 ); - - + SetWatts( 0 ); // stop consuming power Model::Shutdown(); } @@ -401,6 +395,12 @@ stg_laser_sample_t* ModelLaser::GetSamples( uint32_t* count ) { + // get a C style array from our vector if( count ) *count = sample_count; return &samples[0]; } + +const std::vector<stg_laser_sample_t>& ModelLaser::GetSamples() +{ + return samples; +} Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/region.hh 2009-05-13 17:57:36 UTC (rev 7645) @@ -15,7 +15,7 @@ // a bit of experimenting suggests that these values are fast. YMMV. #define RBITS 4 // regions contain (2^RBITS)^2 pixels -#define SBITS 6 // superregions contain (2^SBITS)^2 regions +#define SBITS 5 // superregions contain (2^SBITS)^2 regions #define SRBITS (RBITS+SBITS) #define REGIONWIDTH (1<<RBITS) @@ -107,8 +107,16 @@ void DecrementOccupancy(); void IncrementOccupancy(); -}; + bool Raytrace( int32_t x, int32_t y, + int32_t dx, int32_t dy, + const stg_ray_test_func_t func, + const Model* mod, + const void* arg, + const bool ztest, + stg_raytrace_result_t* res ); + }; + class SuperRegion { friend class World; @@ -141,7 +149,6 @@ void IncrementOccupancy(){ ++count; }; }; - inline void Region::DecrementOccupancy() { assert( superregion ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/stage.hh 2009-05-13 17:57:36 UTC (rev 7645) @@ -42,6 +42,7 @@ #include <iostream> #include <vector> #include <list> +#include <map> //#include <pair> // we use GLib's data structures extensively. Perhaps we'll move to @@ -419,6 +420,10 @@ int x,y; stg_point_int_t( int x, int y ) : x(x), y(y){} stg_point_int_t() : x(0), y(0){} + + /** required to put these in sorted containers like std::map */ + bool operator<( const stg_point_int_t& other ) const + { return ((x < other.x) || (y < other.y) ); } }; @@ -816,15 +821,19 @@ /** raytrace sample */ - typedef struct + class RaytraceResult { + public: Pose pose; ///< location and direction of the ray origin stg_meters_t range; ///< range to beam hit in meters Model* mod; ///< the model struck by this beam stg_color_t color; ///< the color struck by this beam - } stg_raytrace_result_t; + RaytraceResult() : pose(), range(0), mod(NULL), color() {} + }; + typedef RaytraceResult stg_raytrace_result_t; + class Ray { public: @@ -842,7 +851,7 @@ void* arg; bool ztest; - stg_raytrace_result_t result; + RaytraceResult result; }; @@ -923,9 +932,8 @@ GList* powerpack_list; ///< List of all the powerpacks attached to models in the world GList* ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in microseconds - GHashTable* superregions; + std::map<stg_point_int_t,SuperRegion*> superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world - // GList* update_list; ///< Models that have a subscriber or controller, and need to be updated GList* reentrant_update_list; ///< It is safe to call these model's Update() in parallel GList* nonreentrant_update_list; ///< It is NOT safe to call these model's Update() in parallel @@ -996,8 +1004,11 @@ SuperRegion* CreateSuperRegion( stg_point_int_t origin ); void DestroySuperRegion( SuperRegion* sr ); - // trace a vector of rays all in one go + /** trace a vector of rays all in one go. */ void Raytrace( std::vector<Ray>& rays ); + + /** trace a ray. */ + void Raytrace( Ray& ray ); stg_raytrace_result_t Raytrace( const Pose& pose, const stg_meters_t range, @@ -1162,6 +1173,9 @@ void AddToCellArray( GPtrArray* ptrarray ); void RemoveFromCellArray( GPtrArray* ptrarray ); + + //void AddToCellArray( std::vector<Cell*>& blocks ); + //void RemoveFromCellArray( std::vector<Cell*>& blocks ); void GenerateCandidateCells(); @@ -2455,7 +2469,7 @@ }; Vis vis; - + //class LaserRay : public Ray /** OpenGL displaylist for laser data */ @@ -2473,6 +2487,9 @@ std::vector<Ray> rays; + // set up data buffers after the config changes + void SampleConfig(); + public: static const char* typestr; // constructor @@ -2490,15 +2507,18 @@ uint32_t GetSampleCount(){ return sample_count; } - stg_laser_sample_t* GetSamples( uint32_t* count=NULL); - - //void SetSamples( stg_laser_sample_t* samples, uint32_t count); - + /** returns an array of samples */ + stg_laser_sample_t* GetSamples( uint32_t* count ); + + /** returns a const reference to a vector of samples */ + const std::vector<stg_laser_sample_t>& GetSamples(); + // Get the user-tweakable configuration of the laser stg_laser_cfg_t GetConfig( ); // Set the user-tweakable configuration of the laser void SetConfig( stg_laser_cfg_t cfg ); + }; // \todo GRIPPER MODEL -------------------------------------------------------- Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/world.cc 2009-05-13 17:57:36 UTC (rev 7645) @@ -59,17 +59,7 @@ bool World::quit_all = false; GList* World::world_list = NULL; -static guint PointIntHash( stg_point_int_t* pt ) -{ - return( pt->x + (pt->y<<16 )); -} -static gboolean PointIntEqual( stg_point_int_t* p1, stg_point_int_t* p2 ) -{ - return( memcmp( p1, p2, sizeof(stg_point_int_t) ) == 0 ); -} - - World::World( const char* token, stg_msec_t interval_sim, double ppm ) @@ -103,8 +93,9 @@ powerpack_list( NULL ), ray_list( NULL ), sim_time( 0 ), - superregions( g_hash_table_new( (GHashFunc)PointIntHash, - (GEqualFunc)PointIntEqual ) ), + superregions(), + // g_hash_table_new( (GHashFunc)PointIntHash, + // (GEqualFunc)PointIntEqual ) ), sr_cached(NULL), reentrant_update_list( NULL ), nonreentrant_update_list( NULL ), @@ -137,15 +128,14 @@ SuperRegion* World::CreateSuperRegion( stg_point_int_t origin ) { SuperRegion* sr = new SuperRegion( this, origin ); - g_hash_table_insert( superregions, &sr->origin, sr ); - + superregions[ sr->origin ] = sr; dirty = true; // force redraw return sr; } void World::DestroySuperRegion( SuperRegion* sr ) { - g_hash_table_remove( superregions, &sr->origin ); + superregions.erase( sr->origin ); delete sr; } @@ -397,10 +387,11 @@ g_list_free( ray_list ); ray_list = NULL; - - g_hash_table_foreach( superregions, (GHFunc)destroy_sregion, NULL ); - g_hash_table_remove_all( superregions ); + // todo + //g_hash_table_foreach( superregions, (GHFunc)destroy_sregion, NULL ); + //g_hash_table_remove_all( superregions ); + token = NULL; } @@ -614,15 +605,15 @@ void World::Raytrace( std::vector<Ray>& rays ) { - for( std::vector<Ray>::iterator it = rays.begin(); - it != rays.end(); - ++it ) - { - Ray& r = *it; - r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg, r.ztest ); - } + for( unsigned int i=0; i<rays.size(); i++ ) + Raytrace( rays[i] ); } +inline void World::Raytrace( Ray& r ) +{ + r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg, r.ztest ); +} + void World::Raytrace( const Pose &gpose, // global pose const stg_meters_t range, const stg_radians_t fov, @@ -707,7 +698,7 @@ // initialize the sample sample.pose = gpose; - sample.pose.a = normalize( sample.pose.a ); + //sample.pose.a = normalize( sample.pose.a ); sample.range = range; // we might change this below sample.mod = NULL; // we might change this below @@ -722,10 +713,6 @@ // and the x and y offsets of the ray int32_t dx = (int32_t)(ppm*range * cos(gpose.a)); int32_t dy = (int32_t)(ppm*range * sin(gpose.a)); - -// // the number of regions we will travel through -// int32_t rdx = dx / Region::WIDTH; -// int32_t rdy = dy / Region::WIDTH; // if( finder->debug ) // RecordRay( pose.x, @@ -746,16 +733,6 @@ int exy = ay-ax; int n = ax+ay; -// // region units -// int rsx = sgn(rdx); // sgn() is a fast macro -// int rsy = sgn(rdy); -// int rax = abs(rdx); -// int ray = abs(rdy); -// int rbx = 2*rax; -// int rby = 2*ray; -// int rexy = ray-rax; -// int rn = rax+ray; - // printf( "Raytracing from (%d,%d) steps (%d,%d) %d\n", // x,y, dx,dy, n ); @@ -820,13 +797,6 @@ Block* block = *it; assert( block ); - //printf( "ENT %p mod %p z [%.2f %.2f] color %X\n", - // ent, - // ent->mod, - // ent->zbounds.min, - // ent->zbounds.max, - // ent->color ); - // skip if not in the right z range if( ztest && ( gpose.z < block->global_z.min || @@ -851,6 +821,7 @@ sample.range = hypot( (glob.x-start.x)/ppm, (glob.y-start.y)/ppm ); return sample; } + } } } @@ -868,7 +839,7 @@ sup.x = SUPERREGION( glob.x ); if( sup.x != lastsup.x ) { - sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + sr = superregions[ sup ]; lastsup = sup; // remember these coords } @@ -886,7 +857,7 @@ sup.y = SUPERREGION( glob.y ); if( sup.y != lastsup.y ) { - sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + sr = superregions[ sup ]; lastsup = sup; // remember these coords } @@ -965,10 +936,7 @@ inline SuperRegion* World::GetSuperRegion( const stg_point_int_t& sup ) { //printf( "SUP[ %d %d ] ", sup.x, sup.y ); - - // no, so we try to fetch it out of the hash table - SuperRegion* sr = (SuperRegion*) - g_hash_table_lookup( superregions, (gpointer)&sup ); + SuperRegion* sr = superregions[ sup ]; if( sr == NULL ) // no superregion exists! make a new one sr = AddSuperRegion( sup ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstage/worldgui.cc 2009-05-13 17:57:36 UTC (rev 7645) @@ -441,7 +441,12 @@ void WorldGui::DrawTree( bool drawall ) { - g_hash_table_foreach( superregions, (GHFunc)Draw_cb, (void*)drawall ); + //g_hash_table_foreach( superregions, (GHFunc)Draw_cb, (void*)drawall ); + + for( std::map<stg_point_int_t,SuperRegion*>::iterator it = superregions.begin(); + it != superregions.end(); + it++ ) + (*it).second->Draw( drawall ); } // callback wrapper for SuperRegion::Floor() @@ -456,7 +461,13 @@ void WorldGui::DrawFloor() { PushColor( 1,1,1,1 ); - g_hash_table_foreach( superregions, (GHFunc)Floor_cb, NULL ); + //g_hash_table_foreach( superregions, (GHFunc)Floor_cb, NULL ); + + for( std::map<stg_point_int_t,SuperRegion*>::iterator it = superregions.begin(); + it != superregions.end(); + it++ ) + (*it).second->Floor(); + PopColor(); } Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/libstageplugin/p_laser.cc 2009-05-13 17:57:36 UTC (rev 7645) @@ -53,7 +53,7 @@ void InterfaceLaser::Publish( void ) { ModelLaser* mod = (ModelLaser*)this->mod; - stg_laser_sample_t* samples = mod->GetSamples(); + stg_laser_sample_t* samples = mod->GetSamples(NULL); // don't publish anything until we have some real data if( samples == NULL ) Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-05-13 15:41:40 UTC (rev 7644) +++ code/stage/trunk/worlds/simple.world 2009-05-13 17:57:36 UTC (rev 7645) @@ -9,7 +9,7 @@ interval_sim 100 # simulation timestep in milliseconds interval_real 20 # real-time interval between simulation updates in milliseconds -quit_time 60 +quit_time 600 paused 0 @@ -43,7 +43,7 @@ name "r0" pose [ -7 -7 0 45 ] - sicklaser() + sicklaser( samples 361 ) ctrl "wander" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-13 22:13:50
|
Revision: 7649 http://playerstage.svn.sourceforge.net/playerstage/?rev=7649&view=rev Author: rtv Date: 2009-05-13 22:13:42 +0000 (Wed, 13 May 2009) Log Message: ----------- removed grippers from FASR demo Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/webstage/webstage.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-05-13 22:13:42 UTC (rev 7649) @@ -50,7 +50,7 @@ ModelRanger* ranger; ModelFiducial* fiducial; ModelBlobfinder* blobfinder; - ModelGripper* gripper; + //ModelGripper* gripper; Model *source, *sink; int avoidcount, randcount; int work_get, work_put; @@ -70,7 +70,7 @@ ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )), fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )), blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )), - gripper( (ModelGripper*)pos->GetUnusedModelOfType( MODEL_TYPE_GRIPPER )), + //gripper( (ModelGripper*)pos->GetUnusedModelOfType( MODEL_TYPE_GRIPPER )), source(source), sink(sink), avoidcount(0), @@ -104,7 +104,7 @@ fiducial->Subscribe(); //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate, this ); - gripper->Subscribe(); + //gripper->Subscribe(); if( blobfinder ) // optional { @@ -119,12 +119,12 @@ void Dock() { // close the grippers so they can be pushed into the charger - ModelGripper::config_t gripper_data = gripper->GetConfig(); + //ModelGripper::config_t gripper_data = gripper->GetConfig(); - if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) - gripper->CommandClose(); - else if( gripper_data.lift != ModelGripper::LIFT_UP ) - gripper->CommandUp(); +// if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) +// gripper->CommandClose(); +// else if( gripper_data.lift != ModelGripper::LIFT_UP ) +// gripper->CommandUp(); if( charger_ahoy ) { @@ -176,7 +176,7 @@ void UnDock() { - const stg_meters_t gripper_distance = 0.2; + //const stg_meters_t gripper_distance = 0.2; const stg_meters_t back_off_distance = 0.3; const stg_meters_t back_off_speed = -0.05; @@ -187,18 +187,18 @@ pos->SetXSpeed( 0.0 ); // once we have backed off a bit, open and lower the gripper - ModelGripper::config_t gripper_data = gripper->GetConfig(); - if( charger_range > gripper_distance ) - { - if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) - gripper->CommandOpen(); - else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) - gripper->CommandDown(); - } +// ModelGripper::config_t gripper_data = gripper->GetConfig(); +// if( charger_range > gripper_distance ) +// { +// if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) +// gripper->CommandOpen(); +// else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) +// gripper->CommandDown(); +// } // if the gripper is down and open and we're away from the charger, undock is finished - if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && - gripper_data.lift == ModelGripper::LIFT_DOWN && + if( //gripper_data.paddles == ModelGripper::PADDLE_OPEN && + //gripper_data.lift == ModelGripper::LIFT_DOWN && charger_range > back_off_distance ) mode = MODE_WORK; } @@ -288,7 +288,7 @@ { if( verbose ) puts( "Cruise" ); - ModelGripper::config_t gdata = gripper->GetConfig(); + //ModelGripper::config_t gdata = gripper->GetConfig(); //avoidcount = 0; pos->SetXSpeed( cruisespeed ); @@ -307,7 +307,7 @@ if( y < 0 ) y = 0; double a_goal = - dtor( ( pos->GetFlagCount() || gdata.gripped ) ? have[y][x] : need[y][x] ); + dtor( ( pos->GetFlagCount() ) ? have[y][x] : need[y][x] ); // if we are low on juice - find the direction to the recharger instead if( Hungry() ) @@ -324,8 +324,8 @@ { if( ! at_dest ) { - if( gdata.beam[0] ) // inner break beam broken - gripper->CommandClose(); + //if( gdata.beam[0] ) // inner break beam broken + //gripper->CommandClose(); } } @@ -412,7 +412,7 @@ { robot->at_dest = true; - robot->gripper->CommandOpen(); + //robot->gripper->CommandOpen(); if( ++robot->work_put > workduration ) { @@ -469,21 +469,21 @@ return 0; } - static int GripperUpdate( ModelGripper* grip, Robot* robot ) - { - ModelGripper::config_t gdata = grip->GetConfig(); +// static int GripperUpdate( ModelGripper* grip, Robot* robot ) +// { +// ModelGripper::config_t gdata = grip->GetConfig(); - printf( "BREAKBEAMS %s %s\n", - gdata.beam[0] ? gdata.beam[0]->Token() : "<null>", - gdata.beam[1] ? gdata.beam[1]->Token() : "<null>" ); +// printf( "BREAKBEAMS %s %s\n", +// gdata.beam[0] ? gdata.beam[0]->Token() : "<null>", +// gdata.beam[1] ? gdata.beam[1]->Token() : "<null>" ); - printf( "CONTACTS %s %s\n", - gdata.contact[0] ? gdata.contact[0]->Token() : "<null>", - gdata.contact[1] ? gdata.contact[1]->Token() : "<null>"); +// printf( "CONTACTS %s %s\n", +// gdata.contact[0] ? gdata.contact[0]->Token() : "<null>", +// gdata.contact[1] ? gdata.contact[1]->Token() : "<null>"); - return 0; - } +// return 0; +// } static int FlagIncr( Model* mod, Robot* robot ) { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/libstage/model.cc 2009-05-13 22:13:42 UTC (rev 7649) @@ -235,7 +235,7 @@ wf(NULL), wf_entity(0), world(world), - world_gui( dynamic_cast< WorldGui* >( world ) ) + world_gui( dynamic_cast<WorldGui*>( world ) ) { assert( modelsbyid ); assert( world ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/libstage/stage.hh 2009-05-13 22:13:42 UTC (rev 7649) @@ -1204,10 +1204,10 @@ void Rasterize( uint8_t* data, unsigned int width, unsigned int height, stg_meters_t cellwidth, stg_meters_t cellheight ); - + private: Model* mod; ///< model to which this block belongs - + stg_point_t* mpts; ///< cache of this->pts in model coordindates size_t pt_count; ///< the number of points stg_point_t* pts; ///< points defining a polygon @@ -1583,9 +1583,8 @@ virtual void Visualize( Model* mod, Camera* cam ); void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount ); - }; - - DissipationVis event_vis; + } event_vis; + StripPlotVis output_vis; StripPlotVis stored_vis; @@ -1721,30 +1720,6 @@ std::vector<Option*> drawOptions; const std::vector<Option*>& getOptions() const { return drawOptions; } - class RasterVis : public Visualizer - { - private: - uint8_t* data; - unsigned int width, height; - stg_meters_t cellwidth, cellheight; - GList* pts; - - public: - RasterVis(); - virtual ~RasterVis( void ){} - virtual void Visualize( Model* mod, Camera* cam ); - - void SetData( uint8_t* data, - unsigned int width, - unsigned int height, - stg_meters_t cellwidth, - stg_meters_t cellheight ); - - void AddPoint( stg_meters_t x, stg_meters_t y ); - void ClearPts(); - - }; - protected: GMutex* access_mutex; GPtrArray* blinkenlights; @@ -1815,8 +1790,30 @@ GData* props; /** Visualize the most recent rasterization operation performed by this model */ - RasterVis rastervis; + class RasterVis : public Visualizer + { + private: + uint8_t* data; + unsigned int width, height; + stg_meters_t cellwidth, cellheight; + GList* pts; + public: + RasterVis(); + virtual ~RasterVis( void ){} + virtual void Visualize( Model* mod, Camera* cam ); + + void SetData( uint8_t* data, + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ); + + void AddPoint( stg_meters_t x, stg_meters_t y ); + void ClearPts(); + + } rastervis; + bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw char* say_string; ///< if non-null, this string is displayed in the GUI @@ -2009,6 +2006,7 @@ void LoadDataBaseEntries( Worldfile* wf, int entity ); public: + virtual void PushColor( stg_color_t col ) { world->PushColor( col ); } @@ -2466,12 +2464,9 @@ Vis( World* world ); virtual ~Vis( void ){} virtual void Visualize( Model* mod, Camera* cam ); - }; + } vis; - Vis vis; - //class LaserRay : public Ray - /** OpenGL displaylist for laser data */ int data_dl; bool data_dirty; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/libstage/world.cc 2009-05-13 22:13:42 UTC (rev 7649) @@ -258,12 +258,6 @@ //g_hash_table_insert( blockgroups_by_entity, (gpointer)entity, mod->blockgroup ); } -// delete a model from the hash table -static void destroy_sregion( gpointer dummy1, SuperRegion* sr, gpointer dummy2 ) -{ - free(sr); -} - void World::Load( const char* worldfile_path ) { // note: must call Unload() before calling Load() if a world already Modified: code/stage/trunk/webstage/webstage.cc =================================================================== --- code/stage/trunk/webstage/webstage.cc 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/webstage/webstage.cc 2009-05-13 22:13:42 UTC (rev 7649) @@ -55,7 +55,41 @@ printf( "Warning: attempt to push PVA for unrecognized model \"%s\"\n", name.c_str() ); } + + void Push() + { + for( std::map<std::string,Puppet*>::iterator it = puppets.begin(); + it != puppets.end(); + it++ ) + { + Puppet* pup = it->second; + assert(pup); + + Stg::Model* mod = world->GetModel( pup->name.c_str() ); + assert(mod); + + websim::Pose p; + websim::Velocity v; + websim::Acceleration a; + + Stg::Pose sp = mod->GetPose(); + p.x = sp.x; + p.y = sp.y; + p.z = sp.z; + p.a = sp.a; + + Stg::Velocity sv = mod->GetVelocity(); + v.x = sv.x; + v.y = sv.y; + v.z = sv.z; + v.a = sv.a; + + pup->Push( p,v,a ); + printf( "pushing puppet %s\n", pup->name.c_str() ); + } + } + // Interface to be implemented by simulators virtual bool CreateModel(const std::string& name, const std::string& type, @@ -146,11 +180,12 @@ return t; } - static void UpdatePuppetCb( const std::string& name, WebSim::Puppet* pup, void* arg ) - { - WebStage* ws = (WebStage*)arg; - ws->Push( pup->name ); - } +// static void UpdatePuppetCb( const std::string& name, WebSim::Puppet* pup, void* arg ) +// { +// WebStage* ws = (WebStage*)arg; +// ws->Push( pup->name ); +// } + }; @@ -224,9 +259,8 @@ while( ! quit ) { // todo? check for changes? - // send my updates - - ws.ForEachPuppet( WebStage::UpdatePuppetCb, (void*)&ws ); + // send my updates + ws.Push(); //puts( "pushes done" ); // tell my friends to start simulating Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-05-13 20:20:05 UTC (rev 7648) +++ code/stage/trunk/worlds/fasr.world 2009-05-13 22:13:42 UTC (rev 7649) @@ -142,15 +142,16 @@ joules 100000 joules_capacity 400000 fiducial_return 0 - # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) - gripper( pose [0.250 0 -0.220 0] - take_watts 1000.0 - fiducial( range 3 ) - # paddles [ "closed" "up" ] - obstacle_return 0 # cheating for simplicity - # autosnatch 1 - ) + + #gripper( pose [0.250 0 -0.220 0] + # take_watts 1000.0 + # fiducial( range 3 ) + # # paddles [ "closed" "up" ] +# obstacle_return 0 # cheating for simplicity +# # autosnatch 1 +# ) ) autorob( pose [5.488 5.149 0 35.947] joules 300000 name "r0" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-18 21:41:30
|
Revision: 7689 http://playerstage.svn.sourceforge.net/playerstage/?rev=7689&view=rev Author: rtv Date: 2009-05-18 21:40:54 +0000 (Mon, 18 May 2009) Log Message: ----------- very much faster raytracing, with one tiny and infrequent bug that needs hunted Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/examples/ctrl/wander.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/model_gripper.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/benchmark/hospital.world code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/CMakeLists.txt 2009-05-18 21:40:54 UTC (rev 7689) @@ -14,9 +14,9 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF) -add_definitions(-g -Wall) +add_definitions(-O2 -g -Wall) -# todo +# todo - this doesn't work yet. Run Stage headless with -g. # OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) cmake_minimum_required( VERSION 2.4 FATAL_ERROR ) Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -329,13 +329,7 @@ } } - assert( ! isnan(a_goal ) ); - assert( ! isnan(pose.a ) ); - double a_error = normalize( a_goal - pose.a ); - - assert( ! isnan(a_error) ); - pos->SetTurnSpeed( a_error ); } } Modified: code/stage/trunk/examples/ctrl/wander.cc =================================================================== --- code/stage/trunk/examples/ctrl/wander.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/examples/ctrl/wander.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -126,7 +126,7 @@ robot->pos->SetSpeed( 0,0,0 ); robot->pos->SetTurnSpeed( 0 ); } - + return 0; } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/canvas.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -19,7 +19,7 @@ #include <sstream> #include <png.h> - +#include "region.hh" #include "file_manager.hh" #include "options_dlg.hh" @@ -784,6 +784,98 @@ glPopMatrix(); } + if( ! world->rt_cells.empty() ) + { + glPushMatrix(); + GLfloat scale = 1.0/world->Resolution(); + glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly + + world->PushColor( stg_color_pack( 0,0,1,0.5) ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + + for( unsigned int i=0; + i < world->rt_cells.size(); + i++ ) + { +// char str[128]; +// snprintf( str, 128, "(%d,%d)", +// world->rt_cells[i].x, +// world->rt_cells[i].y ); + +// Gl::draw_string( world->rt_cells[i].x+1, +// world->rt_cells[i].y+1, 0.1, str ); + + //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); + glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3, + world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 ); + } + +#if 0 + world->PushColor( stg_color_pack( 0,1,0,0.2) ); + glBegin( GL_LINE_STRIP ); + for( unsigned int i=0; + i < world->rt_cells.size(); + i++ ) + { + glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 ); + } + glEnd(); + world->PopColor(); +#endif + + glPopMatrix(); + world->PopColor(); + + //world->rt_cells.clear(); + } + + if( ! world->rt_candidate_cells.empty() ) + { + glPushMatrix(); + GLfloat scale = 1.0/world->Resolution(); + glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly + + world->PushColor( stg_color_pack( 1,0,0,0.5) ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + for( unsigned int i=0; + i < world->rt_candidate_cells.size(); + i++ ) + { +// char str[128]; +// snprintf( str, 128, "(%d,%d)", +// world->rt_candidate_cells[i].x, +// world->rt_candidate_cells[i].y ); + +// Gl::draw_string( world->rt_candidate_cells[i].x+1, +// world->rt_candidate_cells[i].y+1, 0.1, str ); + + //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); + glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y, + world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 ); + } + + world->PushColor( stg_color_pack( 0,1,0,0.2) ); + glBegin( GL_LINE_STRIP ); + for( unsigned int i=0; + i < world->rt_candidate_cells.size(); + i++ ) + { + glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 ); + } + glEnd(); + world->PopColor(); + + glPopMatrix(); + world->PopColor(); + + //world->rt_cells.clear(); + } + + if( showFootprints ) { glDisable( GL_DEPTH_TEST ); @@ -801,8 +893,8 @@ if( showBBoxes ) DrawBoundingBoxes(); - - + + //LISTMETHOD( world->puck_list, Puck*, Draw ); // TODO - finish this properly Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -13,9 +13,6 @@ //#define DEBUG 1 -#include <assert.h> -#include <math.h> - #include "stage.hh" #include "option.hh" #include "worldfile.hh" Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_getset.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -225,9 +225,9 @@ pose = newpose; pose.a = normalize(pose.a); - if( isnan( pose.a ) ) - printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", - token, pose.x, pose.y, pose.z, pose.a ); +// if( isnan( pose.a ) ) +// printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", +// token, pose.x, pose.y, pose.z, pose.a ); NeedRedraw(); MapWithChildren(); Modified: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_gripper.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -50,7 +50,6 @@ #include <sys/time.h> -#include <math.h> #include "stage.hh" #include "worldfile.hh" using namespace Stg; Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_laser.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -297,13 +297,14 @@ { assert( samples.size() == sample_count ); assert( rays.size() == sample_count ); - + double bearing = -fov/2.0; - double sample_incr = fov / (double)sample_count; - + // make the first and last rays exactly at the extremes of the FOV + double sample_incr = fov / MAX(sample_count-1,1); + Pose rayorg = geom.pose; rayorg.z += geom.size.z/2.0; - rayorg.a = bearing; + rayorg.a = bearing;// + sample_incr/2.0; rayorg = LocalToGlobal(rayorg); // set up the ray origins in global coords Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_position.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -13,11 +13,7 @@ #include <sys/time.h> -#include <math.h> -#include <stdlib.h> -//#define DEBUG - #include "stage.hh" #include "worldfile.hh" using namespace Stg; @@ -468,9 +464,9 @@ void ModelPosition::SetSpeed( double x, double y, double a ) { - assert( ! isnan(x) ); - assert( ! isnan(y) ); - assert( ! isnan(a) ); + //assert( ! isnan(x) ); + //assert( ! isnan(y) ); + //assert( ! isnan(a) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.x = x; @@ -481,7 +477,7 @@ void ModelPosition::SetXSpeed( double x ) { - assert( ! isnan(x) ); + //assert( ! isnan(x) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.x = x; } @@ -489,21 +485,21 @@ void ModelPosition::SetYSpeed( double y ) { - assert( ! isnan(y) ); + //assert( ! isnan(y) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.y = y; } void ModelPosition::SetZSpeed( double z ) { - assert( ! isnan(z) ); + //assert( ! isnan(z) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.z = z; } void ModelPosition::SetTurnSpeed( double a ) { - assert( ! isnan(a) ); + //assert( ! isnan(a) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.a = a; } @@ -511,10 +507,10 @@ void ModelPosition::SetSpeed( Velocity vel ) { - assert( ! isnan(vel.x) ); - assert( ! isnan(vel.y) ); - assert( ! isnan(vel.z) ); - assert( ! isnan(vel.a) ); + //assert( ! isnan(vel.x) ); + //assert( ! isnan(vel.y) ); + //assert( ! isnan(vel.z) ); + //assert( ! isnan(vel.a) ); control_mode = STG_POSITION_CONTROL_VELOCITY; goal.x = vel.x; @@ -525,9 +521,9 @@ void ModelPosition::GoTo( double x, double y, double a ) { - assert( ! isnan(x) ); - assert( ! isnan(y) ); - assert( ! isnan(a) ); + //assert( ! isnan(x) ); + //assert( ! isnan(y) ); + //assert( ! isnan(a) ); control_mode = STG_POSITION_CONTROL_POSITION; goal.x = x; Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_ranger.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -82,8 +82,6 @@ #include "option.hh" using namespace Stg; -#include <math.h> - static const stg_watts_t RANGER_WATTSPERSENSOR = 0.2; static const stg_meters_t RANGER_SIZEX = 0.4; static const stg_meters_t RANGER_SIZEY = 0.4; Modified: code/stage/trunk/libstage/model_wifi.cc =================================================================== --- code/stage/trunk/libstage/model_wifi.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/model_wifi.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -25,7 +25,6 @@ #include <sys/time.h> -#include <math.h> #include "gui.h" // Number pulled directly from my ass Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/region.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -32,8 +32,9 @@ : count(0), origin(origin), world(world) { //static int srcount=0; - //printf( "created SR number %d\n", ++srcount ); - + //printf( "created SR number %d\n", ++srcount ); + // printf( "superregion at %d %d\n", origin.x, origin.y ); + // initialize the parent pointer for all my child regions for( unsigned int i=0; i<SuperRegion::SIZE; i++ ) regions[i].superregion = this; @@ -62,13 +63,13 @@ for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) { - Region* r = GetRegion(x,y); + Region* r = GetRegionLocal(x,y); if( r->count ) // outline regions with contents glRecti( x<<RBITS, y<<RBITS, (x+1)<<RBITS, (y+1)<<RBITS ); - else if( r->cells ) + else// if( r->cells ) { double left = x << RBITS; double right = (x+1) << RBITS; @@ -122,7 +123,7 @@ for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) { - Region* r = GetRegion(x,y); + Region* r = GetRegionLocal( x, y); if( r->count < 1 ) continue; Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/region.hh 2009-05-18 21:40:54 UTC (rev 7689) @@ -5,9 +5,6 @@ Copyright Richard Vaughan 2008 */ -#include <list> // STL containers -// #include <vector> - #include "stage.hh" namespace Stg @@ -35,6 +32,7 @@ private: Region* region; std::list<Block*> blocks; + bool boundary; public: Cell() @@ -45,21 +43,25 @@ inline void RemoveBlock( Block* b ); inline void AddBlock( Block* b ); inline void AddBlockNoRecord( Block* b ); -}; +}; class Region { - friend class SuperRegion; +public: -private: - static const uint32_t WIDTH; - static const uint32_t SIZE; - Cell* cells; SuperRegion* superregion; -public: + static const uint32_t WIDTH; + static const uint32_t SIZE; + + static int32_t CELL( const int32_t x ) + { + const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); + return( x & _cell_coord_mask ); + } + unsigned long count; // number of blocks rendered into these cells Region(); @@ -73,10 +75,10 @@ for( unsigned int i=0; i<Region::SIZE; i++ ) cells[i].region = this; } - return( &cells[x + (y*Region::WIDTH)] ); + return( &cells[CELL(x) + (CELL(y)*Region::WIDTH)] ); } - Cell* GetCellCreate( const stg_point_int_t& c ) + Cell* GetCellGlobalCreate( const stg_point_int_t& c ) { if( ! cells ) { @@ -84,39 +86,32 @@ for( unsigned int i=0; i<Region::SIZE; i++ ) cells[i].region = this; } - return( &cells[c.x + (c.y*Region::WIDTH)] ); + return( &cells[CELL(c.x) + (CELL(c.y)*Region::WIDTH)] ); } - Cell* GetCellNoCreate( int32_t x, int32_t y ) const + Cell* GetCellGlobalNoCreate( int32_t x, int32_t y ) const { - return( &cells[x + (y*Region::WIDTH)] ); + return( &cells[CELL(x) + (CELL(y)*Region::WIDTH)] ); } - Cell* GetCellNoCreate( const stg_point_int_t& c ) const + Cell* GetCellLocallNoCreate( int32_t x, int32_t y ) const { - return( &cells[c.x + (c.y*Region::WIDTH)] ); + return( &cells[x + y*Region::WIDTH] ); } -// Cell* GetCellNoCreateBoundsCheck( const stg_point_int_t& c ) const -// { -// if( c.x < 0 || c.x > WIDTH || c.y < 0 || c.y > WIDTH ) -// return NULL; -// else -// return( &cells[c.x + (c.y*Region::WIDTH)] ); -// } - + + Cell* GetCellGlobalNoCreate( const stg_point_int_t& c ) const + { + return( &cells[CELL(c.x) + (CELL(c.y)*Region::WIDTH)] ); + } + + + void DecrementOccupancy(); void IncrementOccupancy(); +}; + - bool Raytrace( int32_t x, int32_t y, - int32_t dx, int32_t dy, - const stg_ray_test_func_t func, - const Model* mod, - const void* arg, - const bool ztest, - stg_raytrace_result_t* res ); - }; - class SuperRegion { friend class World; @@ -133,14 +128,31 @@ World* world; public: + + static int32_t REGION( const int32_t x ) + { + const int32_t _region_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS ); + return( ( x & _region_coord_mask ) >> RBITS ); + } + SuperRegion( World* world, stg_point_int_t origin ); ~SuperRegion(); - Region* GetRegion( int32_t x, int32_t y ) + Region* GetRegionGlobal( int32_t x, int32_t y ) { + return( ®ions[ REGION(x) + (REGION(y)*SuperRegion::WIDTH) ] ); + } + + Region* GetRegionLocal( int32_t x, int32_t y ) + { return( ®ions[ x + (y*SuperRegion::WIDTH) ] ); - }; + } + + Region* GetRegionGlobal( const stg_point_int_t& r ) + { + return( ®ions[ REGION(r.x) + (REGION(r.y)*SuperRegion::WIDTH) ] ); + } void Draw( bool drawall ); void Floor(); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/stage.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -1,21 +1,8 @@ -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <sys/types.h> -#include <errno.h> -#include <sys/socket.h> -#include <netdb.h> -#include <netinet/in.h> -#include <assert.h> -#include <unistd.h> -#include <math.h> -#include <glib.h> -#include <locale.h> +// Author: Richard Vaughan +#include <errno.h> #include <FL/Fl_Shared_Image.H> -//#define DEBUG - #include "stage.hh" #include "config.h" // results of cmake's system configuration tests #include "file_manager.hh" @@ -53,7 +40,6 @@ return init_called; } - void Stg::RegisterModel( stg_model_type_t type, const char* name, stg_creator_t creator ) @@ -71,7 +57,6 @@ } - void Stg::stg_print_velocity( const Velocity& vel ) { printf( "velocity [x:%.3f y:%.3f a:%.3f]\n", Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/stage.hh 2009-05-18 21:40:54 UTC (rev 7689) @@ -30,15 +30,18 @@ * SVN: $Id$ */ +// C libs #include <unistd.h> #include <stdint.h> // for portable int types eg. uint32_t #include <assert.h> -#include <math.h> #include <stdlib.h> #include <stdio.h> #include <string.h> #include <sys/types.h> #include <sys/time.h> + +// C++ libs +#include <cmath> #include <iostream> #include <vector> #include <list> @@ -402,6 +405,9 @@ stg_meters_t x, y; stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){} stg_point_t() : x(0.0), y(0.0){} + + bool operator+=( const stg_point_t& other ) + { return ((x += other.x) && (y += other.y) ); } }; /** Define a point in 3d space */ @@ -424,6 +430,9 @@ /** required to put these in sorted containers like std::map */ bool operator<( const stg_point_int_t& other ) const { return ((x < other.x) || (y < other.y) ); } + + bool operator==( const stg_point_int_t& other ) const + { return ((x == other.x) && (y == other.y) ); } }; @@ -828,8 +837,11 @@ stg_meters_t range; ///< range to beam hit in meters Model* mod; ///< the model struck by this beam stg_color_t color; ///< the color struck by this beam - + RaytraceResult() : pose(), range(0), mod(NULL), color() {} + RaytraceResult( const Pose& pose, + stg_meters_t range ) + : pose(pose), range(range), mod(NULL), color() {} }; typedef RaytraceResult stg_raytrace_result_t; @@ -944,6 +956,10 @@ void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true; public: + + // std::vector<stg_point_int_t> rt_regions; + std::vector<stg_point_int_t> rt_cells; + std::vector<stg_point_int_t> rt_candidate_cells; static const int DEFAULT_PPM = 50; // default resolution in pixels per meter static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep @@ -955,7 +971,6 @@ /** Remove a callback function. Any argument data passed to AddUpdateCallback is not automatically freed. */ int RemoveUpdateCallback( stg_world_callback_t cb, void* user ); - /** Log the state of a Model */ void Log( Model* mod ); @@ -977,9 +992,12 @@ SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord); + SuperRegion* GetSuperRegionCached( int32_t x, int32_t y ); void ExpireSuperRegion( SuperRegion* sr ); - inline Cell* GetCell( const int32_t x, const int32_t y ); + inline Cell* GetCellNoCreate( const stg_point_int_t& glob ); + inline Cell* GetCellNoCreate( const int32_t x, const int32_t y ); + inline Cell* GetCellCreate( const int32_t x, const int32_t y ); void ForEachCellInPolygon( const stg_point_t pts[], const unsigned int pt_count, @@ -1135,6 +1153,22 @@ ~Block(); + +// bool RayTest( const stg_ray_test_func_t func, +// const Model* mod, +// const void* arg, +// const bool ztest, +// const stg_meters_t z ) +// { +// // optionally test z is in right range +// if( ztest && ( z < global_z.min || +// z > global_z.max ) ) +// return false; + +// // test the predicate we were passed +// return( (*func)( this->mod, (Model*)mod, arg )); +// } + /** render the block into the world's raytrace data structure */ void Map(); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/world.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -599,8 +599,15 @@ void World::Raytrace( std::vector<Ray>& rays ) { - for( unsigned int i=0; i<rays.size(); i++ ) - Raytrace( rays[i] ); + rt_cells.clear(); + rt_candidate_cells.clear(); + + //printf( "===================\n" ); + + for( std::vector<Ray>::iterator it = rays.begin(); + it != rays.end(); + ++it ) + Raytrace( *it ); } inline void World::Raytrace( Ray& r ) @@ -629,51 +636,12 @@ } } +// fast macros for converting from global cell coordinates to local coords +#define GETCELL(X) (((int32_t)X) & ~((~0x00)<<RBITS )) +#define GETREG(X) ((((int32_t)X) & ~((~0x00)<<SRBITS ))>>RBITS) +#define GETSREG(X) (((int32_t)X)>>SRBITS) -// inline functions for converting from global cell coordinates to -// superregions, regions and local cells - -inline int32_t SUPERREGION( const int32_t x ) -{ - return( x >> SRBITS ); -} - -inline int32_t REGION( const int32_t x ) -{ - const int32_t _region_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS ); - return( ( x & _region_coord_mask ) >> RBITS ); -} - -inline int32_t CELL( const int32_t x ) -{ - const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); - return( x & _cell_coord_mask ); -} - -inline stg_point_int_t SUPERREGION( const stg_point_int_t& glob ) -{ - stg_point_int_t sr; - sr.x = SUPERREGION( glob.x ); - sr.y = SUPERREGION( glob.y ); - return sr; -} - -inline stg_point_int_t REGION( const stg_point_int_t& glob ) -{ - stg_point_int_t reg; - reg.x = REGION( glob.x ); - reg.y = REGION( glob.y ); - return reg; -} - -inline stg_point_int_t CELL( const stg_point_int_t& glob ) -{ - stg_point_int_t c; - c.x = CELL( glob.x ); - c.y = CELL( glob.y ); - return c; -} - +// Stage spends 50-99% of its time in this method. stg_raytrace_result_t World::Raytrace( const Pose &gpose, const stg_meters_t range, const stg_ray_test_func_t func, @@ -681,188 +649,249 @@ const void* arg, const bool ztest ) { - stg_raytrace_result_t sample; + rt_cells.clear(); + rt_candidate_cells.clear(); - // printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n", - // gpose.x, - // gpose.y, - // gpose.z, - // gpose.a, - // range ); - // initialize the sample - sample.pose = gpose; - //sample.pose.a = normalize( sample.pose.a ); - sample.range = range; // we might change this below - sample.mod = NULL; // we might change this below + RaytraceResult sample( gpose, range ); - // find the global integer bitmap address of the ray - stg_point_int_t glob; - glob.x = (int32_t)(gpose.x*ppm); - glob.y = (int32_t)(gpose.y*ppm); - + // our global position in (floating point) cell coordinates + stg_point_t glob( gpose.x * ppm, gpose.y * ppm ); + // record our starting position - stg_point_int_t start = glob; - + const stg_point_int_t start( glob.x, glob.y ); + + // eliminate a potential divide by zero + const double angle( gpose.a == 0.0 ? 1e-12 : gpose.a ); + const double cosa(cos(angle)); + const double sina(sin(angle)); + const double tana(sina/cosa); // = tan(angle) + // and the x and y offsets of the ray - int32_t dx = (int32_t)(ppm*range * cos(gpose.a)); - int32_t dy = (int32_t)(ppm*range * sin(gpose.a)); - - // if( finder->debug ) - // RecordRay( pose.x, - // pose.y, - // pose.x + range.max * cos(pose.a), - // pose.y + range.max * sin(pose.a) ); - + const int dx( ppm * range * cosa); + const int dy( ppm * range * sina); + // fast integer line 3d algorithm adapted from Cohen's code from - // Graphics Gems IV + // Graphics Gems IV + const int sx(sgn(dx)); // sgn() is a fast macro + const int sy(sgn(dy)); + const int ax(abs(dx)); + const int ay(abs(dy)); + const int bx(2*ax); + const int by(2*ay); + int exy(ay-ax); + int n(ax+ay); // the manhattan distance to the goal cell + + const int rsize( Region::WIDTH ); + + // fix a little issue where rays are not drawn long enough when + // drawing to the right or up + if( (dx > 0) || ( dy > 0 ) ) + n++; + + // the distances between region crossings in X and Y + const double xjumpx( sx * rsize ); + const double xjumpy( sx * rsize * tana ); + const double yjumpx( sy * rsize / tana ); + const double yjumpy( sy * rsize ); + // manhattan distance between region crossings in X and Y + const double xjumpdist( fabs(xjumpx)+fabs(xjumpy) ); + const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) ); - // cell unti - int sx = sgn(dx); // sgn() is a fast macro - int sy = sgn(dy); - int ax = abs(dx); - int ay = abs(dy); - int bx = 2*ax; - int by = 2*ay; - int exy = ay-ax; - int n = ax+ay; + // these are updated as we go along the ray + double xcrossx(0), xcrossy(0); + double ycrossx(0), ycrossy(0); + double distX(0), distY(0); + bool calculatecrossings( true ); + - // printf( "Raytracing from (%d,%d) steps (%d,%d) %d\n", - // x,y, dx,dy, n ); - - // superregion coords - stg_point_int_t lastsup( INT_MAX, INT_MAX ); - stg_point_int_t lastreg( INT_MAX, INT_MAX ); - - SuperRegion* sr = NULL; - Region* r = NULL; - bool nonempty_region = false; - - // superregion coords (must be updated every time x or y changes - // but only one variable changes at a time in the loop, so its - // more efficient to compute at the end of the loop, when we know what's changed) - stg_point_int_t sup = SUPERREGION( glob ); + // puts( "=======================" ); - // find the region coords inside this superregion (again: only updated when x or y changes) - stg_point_int_t reg = REGION( glob ); - - // compute the pixel offset inside this region (again: only updated when x or y changes) - stg_point_int_t cell = CELL( glob ); - - // fix a little issue where rays are not drawn long enough when - // drawing to the right or up - if( dx > 0 ) - n++; - else if( dy > 0 ) - n++; - - // find the starting superregion - sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely - - while ( n-- ) - { - //printf( "pixel [%d %d]\tS[ %d %d ]\t", - // x, y, sup.x, sup.y ); - - if( sr ) - { - // printf( "R[ %d %d ]\t", reg.x, reg.y ); + // Stage spends up to 95% of its time in this loop! It would be + // neater with more function calls encapsualting things, but even + // inline calls have a noticeable (2-3%) effect on performance + while( n > 0 ) // while we are still not at the ray end + { + SuperRegion* sr = + GetSuperRegionCached(GETSREG(glob.x), + GETSREG(glob.y)); + + // coordinates of the region inside the superregion + int32_t rx = GETREG(glob.x); + int32_t ry = GETREG(glob.y); + Region* r = &sr->regions[ rx + (ry*SuperRegion::WIDTH) ]; + + if( r->count ) // if the region contains any objects + { + // invalidate the region crossing points used to jump over + // empty regions + calculatecrossings = true; - // if the region coordinate has changed since the last loop - if( reg.x != lastreg.x || reg.y != lastreg.y ) - { - // lookup the new region - r = sr->GetRegion( reg.x, reg.y ); - lastreg = reg; - nonempty_region = ( r && r->count ); - } - - if( nonempty_region ) - { - //printf( "C[ %d %d ]\t", cell.x, cell.y ); + // convert from global cell to local cell coords + int32_t cx = GETCELL(glob.x); + int32_t cy = GETCELL(glob.y); + + Cell* c = &r->cells[ cx + cy * Region::WIDTH ]; + assert(c); // should be a cell there + + // while within the bounds of this region and while some ray remains + // we'll tweak the cell pointer directly to move around quickly + while( (cx>=0) && (cx<(int)Region::WIDTH) && + (cy>=0) && (cy<(int)Region::WIDTH) && + n > 0 ) + { + //printf( "cx %d cy %d\n", cx, cy ); - Cell* c = r->GetCellNoCreate( cell.x, cell.y ); - assert(c); - for( std::list<Block*>::iterator it = c->blocks.begin(); it != c->blocks.end(); ++it ) { Block* block = *it; - assert( block ); - + // skip if not in the right z range if( ztest && ( gpose.z < block->global_z.min || - gpose.z > block->global_z.max ) ) - { - //max( "failed ztest: ray at %.2f block at [%.2f %.2f]\n", - // pose.z, ent->zbounds.min, ent->zbounds.max ); - continue; - } - - //printf( "RT: mod %p testing mod %p at %d %d\n", - // mod, ent->mod, x, y ); - // printf( "RT: mod %p %s testing mod %p %s at %d %d\n", - // mod, mod->Token(), ent->mod, ent->mod->Token(), x, y ); - + gpose.z > block->global_z.max ) ) + continue; + // test the predicate we were passed if( (*func)( block->mod, (Model*)mod, arg )) { // a hit! sample.color = block->GetColor(); sample.mod = block->mod; - sample.range = hypot( (glob.x-start.x)/ppm, (glob.y-start.y)/ppm ); + + if( ax > ay ) // faster than the equivalent hypot() call + sample.range = fabs((glob.x-start.x) / cosa) / ppm; + else + sample.range = fabs((glob.y-start.y) / sina) / ppm; + return sample; - } - + } } + + // increment our cell in the correct direction + if( exy < 0 ) // we're iterating along X + { + glob.x += sx; // global coordinate + exy += by; + c += sx; // move the cell left or right + cx += sx; // cell coordinate for bounds checking + } + else // we're iterating along Y + { + glob.y += sy; // global coordinate + exy -= bx; + c += sy * Region::WIDTH; // move the cell up or down + cy += sy; // cell coordinate for bounds checking + } + n--; // decrement the manhattan distance remaining + + //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); } - } - - // printf( "\t step %d n %d pixel [ %d, %d ] block [ %d %d ] index [ %d %d ] \n", - // //coarse [ %d %d ]\n", - // count++, n, x, y, blockx, blocky, b_dx, b_dy ); - - // increment our pixel in the correct direction - if( exy < 0 ) // we're iterating along X - { - glob.x += sx; - exy += by; - - sup.x = SUPERREGION( glob.x ); - if( sup.x != lastsup.x ) + + //printf( "leaving populated region\n" ); + } + else // jump over the empty region + { + // on the first run, and when we've been iterating over cells, + // we need to calculate the next crossing of region in each + // axis + if( calculatecrossings ) { - sr = superregions[ sup ]; - lastsup = sup; // remember these coords + calculatecrossings = false; + + // find the coordinate in cells of the bottom left corner of + // the current region + int ix = glob.x; + int iy = glob.y; + double regionx = ix/rsize*rsize; + double regiony = iy/rsize*rsize; + if( (glob.x < 0) && (ix % rsize) ) regionx -= rsize; + if( (glob.y < 0) && (iy % rsize) ) regiony -= rsize; + + //double regionx = glob.x - fmod(glob.x,rsize); + //double regiony = glob.y - fmod(glob.y,rsize); + + //printf( "region %.2f %.2f\n", regionx, regiony ); + + // calculate the distance to the edge of the current region + double xdx = sx < 0 ? + regionx - glob.x - 1.0 : // going left + regionx + rsize - glob.x; // going right + double xdy = xdx*tana; + + double ydy = sy < 0 ? + regiony - glob.y - 1.0 : // going down + regiony + rsize - glob.y; // going up + double ydx = ydy/tana; + + // these stored hit points are updated as we go along + xcrossx = glob.x+xdx; + xcrossy = glob.y+xdy; + + ycrossx = glob.x+ydx; + ycrossy = glob.y+ydy; + + // find the distances to the region crossing points + // manhattan distance is faster than using hypot() + distX = fabs(xdx)+fabs(xdy); + distY = fabs(ydx)+fabs(ydy); } - // recompute current region and cell (we can skip these if - // sr==NULL, but profiling suggests it's faster to do them - // than add a conditional) - reg.x = REGION( glob.x); - cell.x = CELL( glob.x ); - } - else // we're iterating along Y - { - glob.y += sy; - exy -= bx; +// printf( "globx %.2f globy %.2f\n", glob.x, glob.y ); +// printf( "xcross (%.2f,%.2f) ycross(%.2f,%.2f)\n", xcrossx, xcrossy, ycrossx, ycrossy ); +// printf( "distX %.2f distY %.2f\n", distX, distY ); +// printf( "xjumpdist %.2f yjumpdist %.2f\n", xjumpdist, yjumpdist ); +// puts( "" ); - sup.y = SUPERREGION( glob.y ); - if( sup.y != lastsup.y ) + if( distX < distY ) // crossing a region boundary left or right { - sr = superregions[ sup ]; - lastsup = sup; // remember these coords - } - - // recompute current region and cell (we can skip these if - // sr==NULL, but profiling suggests it's faster to do them - // than add a conditional) - reg.y = REGION( glob.y ); - cell.y = CELL( glob.y ); - } - } - + //puts( "distX" ); + // move to the X crossing + glob.x = xcrossx; + glob.y = xcrossy; + + n -= distX; // decrement remaining manhattan distance + + // calculate the next region crossing + xcrossx += xjumpx; + xcrossy += xjumpy; + + distY -= distX; + distX = xjumpdist; + + //rt_candidate_cells.push_back( stg_point_int_t( xcrossx, xcrossy )); + } + else // crossing a region boundary up or down + { + //puts( "distY" ); + // move to the X crossing + glob.x = ycrossx; + glob.y = ycrossy; + + n -= distY; // decrement remaining manhattan distance + + // calculate the next region crossing + ycrossx += yjumpx; + ycrossy += yjumpy; + + distX -= distY; + distY = yjumpdist; + + //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); + } + +// if( (GETCELL(xcrossx) == GETCELL(ycrossx) ) && +// (GETCELL(xcrossy) == GETCELL(ycrossy) ) ) +// printf( "SAME %d=%d %d=%d\n", +// GETCELL(xcrossx), GETCELL(ycrossx), +// GETCELL(xcrossy), GETCELL(ycrossy) ); + + //printf( "jumped to glob (%.2f %.2f)\n", glob.x, glob.y ); + } + //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); + } // hit nothing sample.mod = NULL; return sample; @@ -916,17 +945,31 @@ } + +inline SuperRegion* World::GetSuperRegionCached( int32_t x, int32_t y ) +{ + // around 99% of the time the SR is the same as last + // lookup - cache gives a 4% overall speed up :) + + if( sr_cached && sr_cached->origin.x == x && sr_cached->origin.y == y ) + return sr_cached; + //else + // delay constructing the object as long as possible + return GetSuperRegion( stg_point_int_t(x,y) ); +} + inline SuperRegion* World::GetSuperRegionCached( const stg_point_int_t& sup ) { // around 99% of the time the SR is the same as last // lookup - cache gives a 4% overall speed up :) - if( sr_cached && sr_cached->origin.x == sup.x && sr_cached->origin.y == sup.y ) + if( sr_cached && sr_cached->origin == sup ) return sr_cached; //else return GetSuperRegion( sup); } + inline SuperRegion* World::GetSuperRegion( const stg_point_int_t& sup ) { //printf( "SUP[ %d %d ] ", sup.x, sup.y ); @@ -942,17 +985,25 @@ return sr; } -inline Cell* World::GetCell( const int32_t x, const int32_t y ) +inline Cell* World::GetCellNoCreate( const stg_point_int_t& glob ) { - stg_point_int_t glob; - glob.x = x; - glob.y = y; + Region* r = GetSuperRegionCached( GETSREG(glob.x), GETSREG(glob.y) ) + ->GetRegionGlobal( glob ); + if( r->count ) + return r->GetCellGlobalNoCreate( glob ) ; + return NULL; +} + +inline Cell* World::GetCellCreate( const int32_t x, const int32_t y ) +{ + stg_point_int_t glob( x, y ); + //printf( "GC[ %d %d ] ", glob.x, glob.y ); - - return( GetSuperRegionCached( SUPERREGION(glob) ) - ->GetRegion( REGION(x), REGION(y) ) - ->GetCellCreate( CELL(x), CELL(y) )) ; + + return( GetSuperRegionCached( GETSREG(x), GETSREG(y) ) + ->GetRegionGlobal( glob ) + ->GetCellGlobalCreate( glob )) ; } void World::ForEachCellInPolygon( const stg_point_t pts[], @@ -970,23 +1021,23 @@ stg_cell_callback_t cb, void* cb_arg ) { - int32_t x = MetersToPixels( pt1.x ); // global pixel coords - int32_t y = MetersToPixels( pt1.y ); + int x = MetersToPixels( pt1.x ); // global pixel coords + int y = MetersToPixels( pt1.y ); - int32_t dx = MetersToPixels( pt2.x - pt1.x ); - int32_t dy = MetersToPixels( pt2.y - pt1.y ); + int dx = MetersToPixels( pt2.x - pt1.x ); + int dy = MetersToPixels( pt2.y - pt1.y ); // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. - int sx = sgn(dx); // sgn() is a fast macro - int sy = sgn(dy); - int ax = abs(dx); - int ay = abs(dy); - int bx = 2*ax; - int by = 2*ay; - int exy = ay-ax; - int n = ax+ay; + const int sx(sgn(dx)); + const int sy(sgn(dy)); + const int ax(abs(dx)); + const int ay(abs(dy)); + const int bx(2*ax); + const int by(2*ay); + int exy(ay-ax); + int n(ax+ay); // fix a little issue where the edges are not drawn long enough // when drawing to the right or up @@ -995,9 +1046,9 @@ while( n-- ) { - // find the cell at this location, then call the callback + // find or create the cell at this location, then call the callback // with the cell, block and user-defined argument - (*cb)( GetCell( x,y ), cb_arg ); + (*cb)( GetCellCreate( x,y ), cb_arg ); // cleverly skip to the next cell if( exy < 0 ) @@ -1090,3 +1141,4 @@ //LogEntry::Print(); } + Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/libstage/worldgui.cc 2009-05-18 21:40:54 UTC (rev 7689) @@ -429,16 +429,6 @@ return s; } - -// callback wrapper for SuperRegion::Draw() -static void Draw_cb( gpointer dummykey, - SuperRegion* sr, - bool drawall ) -{ - sr->Draw( drawall ); -} - - void WorldGui::DrawTree( bool drawall ) { //g_hash_table_foreach( superregions, (GHFunc)Draw_cb, (void*)drawall ); @@ -449,15 +439,6 @@ (*it).second->Draw( drawall ); } -// callback wrapper for SuperRegion::Floor() -static void Floor_cb( gpointer dummykey, - SuperRegion* sr, - gpointer dummyval ) -{ - sr->Floor(); -} - - void WorldGui::DrawFloor() { PushColor( 1,1,1,1 ); @@ -471,8 +452,6 @@ PopColor(); } - - void WorldGui::windowCb( Fl_Widget* w, void* p ) { WorldGui* worldGui = static_cast<WorldGui*>( p ); Modified: code/stage/trunk/worlds/benchmark/hospital.world =================================================================== --- code/stage/trunk/worlds/benchmark/hospital.world 2009-05-18 18:26:48 UTC (rev 7688) +++ code/stage/trunk/worlds/benchmark/hospital.world 2009-05-18 21:40:54 UTC (rev 7689) @@ -1062,1003 +1062,1003 @@ swarmbot( name "r998" pose [ -9.250 -14.250 0 0 ] ) swarmbot( name "r999" pose [ -9.250 -14.000 0 0 ] ) -swarmbot( name "r1000" pose [ 0 -15.000 0 0 ] ) -swarmbot( name "r1001" pose [ 0 -14.750 0 0 ] ) -swarmbot( name "r1002" pose [ 0 -14.500 0 0 ] ) -swarmbot( name "r1003" pose [ 0 -14.250 0 0 ] ) -swarmbot( name "r1004" pose [ 0 -14.000 0 0 ] ) -swarmbot( name "r1005" pose [ .250 -15.000 0 0 ] ) -swarmbot( name "r1006" pose [ .250 -14.750 0 0 ] ) -swarmbot( name "r1007" pose [ .250 -14.500 0 0 ] ) -swarmbot( name "r1008" pose [ .250 -14.250 0 0 ] ) -swarmbot( name "r1009" pose [ .250 -14.000 0 0 ] ) -swarmbot( name "r1010" pose [ .500 -15.000 0 0 ] ) -swarmbot( name "r1011" pose [ .500 -14.750 0 0 ] ) -swarmbot( name "r1012" pose [ .500 -14.500 0 0 ] ) -swarmbot( name "r1013" pose [ .500 -14.250 0 0 ] ) -swarmbot( name "r1014" pose [ .500 -14.000 0 0 ] ) -swarmbot( name "r1015" pose [ .750 -15.000 0 0 ] ) -swarmbot( name "r1016" pose [ .750 -14.750 0 0 ] ) -swarmbot( name "r1017" pose [ .750 -14.500 0 0 ] ) -swarmbot( name "r1018" pose [ .750 -14.250 0 0 ] ) -swarmbot( name "r1019" pose [ .750 -14.000 0 0 ] ) -swarmbot( name "r1020" pose [ 1.000 -15.000 0 0 ] ) -swarmbot( name "r1021" pose [ 1.000 -14.750 0 0 ] ) -swarmbot( name "r1022" pose [ 1.000 -14.500 0 0 ] ) -swarmbot( name "r1023" pose [ 1.000 -14.250 0 0 ] ) -swarmbot( name "r1024" pose [ 1.000 -14.000 0 0 ] ) -swarmbot( name "r1025" pose [ 1.250 -15.000 0 0 ] ) -swarmbot( name "r1026" pose [ 1.250 -14.750 0 0 ] ) -swarmbot( name "r1027" pose [ 1.250 -14.500 0 0 ] ) -swarmbot( name "r1028" pose [ 1.250 -14.250 0 0 ] ) -swarmbot( name "r1029" pose [ 1.250 -14.000 0 0 ] ) -swarmbot( name "r1030" pose [ 1.500 -15.000 0 0 ] ) -swarmbot( name "r1031" pose [ 1.500 -14.750 0 0 ] ) -swarmbot( name "r1032" pose [ 1.500 -14.500 0 0 ] ) -swarmbot( name "r1033" pose [ 1.500 -14.250 0 0 ] ) -swarmbot( name "r1034" pose [ 1.500 -14.000 0 0 ] ) -swarmbot( name "r1035" pose [ 1.750 -15.000 0 0 ] ) -swarmbot( name "r1036" pose [ 1.750 -14.750 0 0 ] ) -swarmbot( name "r1037" pose [ 1.750 -14.500 0 0 ] ) -swarmbot( name "r1038" pose [ 1.750 -14.250 0 0 ] ) -swarmbot( name "r1039" pose [ 1.750 -14.000 0 0 ] ) -swarmbot( name "r1040" pose [ 2.000 -15.000 0 0 ] ) -swarmbot( name "r1041" pose [ 2.000 -14.750 0 0 ] ) -swarmbot( name "r1042" pose [ 2.000 -14.500 0 0 ] ) -swarmbot( name "r1043" pose [ 2.000 -14.250 0 0 ] ) -swarmbot( name "r1044" pose [ 2.000 -14.000 0 0 ] ) -swarmbot( name "r1045" pose [ 2.250 -15.000 0 0 ] ) -swarmbot( name "r1046" pose [ 2.250 -14.750 0 0 ] ) -swarmbot( name "r1047" pose [ 2.250 -14.500 0 0 ] ) -swarmbot( name "r1048" pose [ 2.250 -14.250 0 0 ] ) -swarmbot( name "r1049" pose [ 2.250 -14.000 0 0 ] ) -swarmbot( name "r1050" pose [ 2.500 -15.000 0 0 ] ) -swarmbot( name "r1051" pose [ 2.500 -14.750 0 0 ] ) -swarmbot( name "r1052" pose [ 2.500 -14.500 0 0 ] ) -swarmbot( name "r1053" pose [ 2.500 -14.250 0 0 ] ) -swarmbot( name "r1054" pose [ 2.500 -14.000 0 0 ] ) -swarmbot( name "r1055" pose [ 2.750 -15.000 0 0 ] ) -swarmbot( name "r1056" pose [ 2.750 -14.750 0 0 ] ) -swarmbot( name "r1057" pose [ 2.750 -14.500 0 0 ] ) -swarmbot( name "r1058" pose [ 2.750 -14.250 0 0 ] ) -swarmbot( name "r1059" pose [ 2.750 -14.000 0 0 ] ) -swarmbot( name "r1060" pose [ 3.000 -15.000 0 0 ] ) -swarmbot( name "r1061" pose [ 3.000 -14.750 0 0 ] ) -swarmbot( name "r1062" pose [ 3.000 -14.500 0 0 ] ) -swarmbot( name "r1063" pose [ 3.000 -14.250 0 0 ] ) -swarmbot( name "r1064" pose [ 3.000 -14.000 0 0 ] ) -swarmbot( name "r1065" pose [ 3.250 -15.000 0 0 ] ) -swarmbot( name "r1066" pose [ 3.250 -14.750 0 0 ] ) -swarmbot( name "r1067" pose [ 3.250 -14.500 0 0 ] ) -swarmbot( name "r1068" pose [ 3.250 -14.250 0 0 ] ) -swarmbot( name "r1069" pose [ 3.250 -14.000 0 0 ] ) -swarmbot( name "r1070" pose [ 3.500 -15.000 0 0 ] ) -swarmbot( name "r1071" pose [ 3.500 -14.750 0 0 ] ) -swarmbot( name "r1072" pose [ 3.500 -14.500 0 0 ] ) -swarmbot( name "r1073" pose [ 3.500 -14.250 0 0 ] ) -swarmbot( name "r1074" pose [ 3.500 -14.000 0 0 ] ) -swarmbot( name "r1075" pose [ 3.750 -15.000 0 0 ] ) -swarmbot( name "r1076" pose [ 3.750 -14.750 0 0 ] ) -swarmbot( name "r1077" pose [ 3.750 -14.500 0 0 ] ) -swarmbot( name "r1078" pose [ 3.750 -14.250 0 0 ] ) -swarmbot( name "r1079" pose [ 3.750 -14.000 0 0 ] ) -swarmbot( name "r1080" pose [ 4.000 -15.000 0 0 ] ) -swarmbot( name "r1081" pose [ 4.000 -14.750 0 0 ] ) -swarmbot( name "r1082" pose [ 4.000 -14.500 0 0 ] ) -swarmbot( name "r1083" pose [ 4.000 -14.250 0 0 ] ) -swarmbot( name "r1084" pose [ 4.000 -14.000 0 0 ] ) -swarmbot( name "r1085" pose [ 4.250 -15.000 0 0 ] ) -swarmbot( name "r1086" pose [ 4.250 -14.750 0 0 ] ) -swarmbot( name "r1087" pose [ 4.250 -14.500 0 0 ] ) -swarmbot( name "r1088" pose [ 4.250 -14.250 0 0 ] ) -swarmbot( name "r1089" pose [ 4.250 -14.000 0 0 ] ) -swarmbot( name "r1090" pose [ 4.500 -15.000 0 0 ] ) -swarmbot( name "r1091" pose [ 4.500 -14.750 0 0 ] ) -swarmbot( name "r1092" pose [ 4.500 -14.500 0 0 ] ) -swarmbot( name "r1093" pose [ 4.500 -14.250 0 0 ] ) -swarmbot( name "r1094" pose [ 4.500 -14.000 0 0 ] ) -swarmbot( name "r1095" pose [ 4.750 -15.000 0 0 ] ) -swarmbot( name "r1096" pose [ 4.750 -14.750 0 0 ] ) -swarmbot( name "r1097" pose [ 4.750 -14.500 0 0 ] ) -swarmbot( name "r1098" pose [ 4.750 -14.250 0 0 ] ) -swarmbot( name "r1099" pose [ 4.750 -14.000 0 0 ] ) -swarmbot( name "r1100" pose [ 5.000 -15.000 0 0 ] ) -swarmbot( name "r1101" pose [ 5.000 -14.750 0 0 ] ) -swarmbot( name "r1102" pose [ 5.000 -14.500 0 0 ] ) -swarmbot( name "r1103" pose [ 5.000 -14.250 0 0 ] ) -swarmbot( name "r1104" pose [ 5.000 -14.000 0 0 ] ) -swarmbot( name "r1105" pose [ 5.250 -15.000 0 0 ] ) -swarmbot( name "r1106" pose [ 5.250 -14.750 0 0 ] ) -swarmbot( name "r1107" pose [ 5.250 -14.500 0 0 ] ) -swarmbot( name "r1108" pose [ 5.250 -14.250 0 0 ] ) -swarmbot( name "r1109" pose [ 5.250 -14.000 0 0 ] ) -swarmbot( name "r1110" pose [ 5.500 -15.000 0 0 ] ) -swarmbot( name "r1111" pose [ 5.500 -14.750 0 0 ] ) -swarmbot( name "r1112" pose [ 5.500 -14.500 0 0 ] ) -swarmbot( name "r1113" pose [ 5.500 -14.250 0 0 ] ) -swarmbot( name "r1114" pose [ 5.500 -14.000 0 0 ] ) -swarmbot( name "r1115" pose [ 5.750 -15.000 0 0 ] ) -swarmbot( name "r1116" pose [ 5.750 -14.750 0 0 ] ) -swarmbot( name "r1117" pose [ 5.750 -14.500 0 0 ] ) -swarmbot( name "r1118" pose [ 5.750 -14.250 0 0 ] ) -swarmbot( name "r1119" pose [ 5.750 -14.000 0 0 ] ) -swarmbot( name "r1120" pose [ 6.000 -15.000 0 0 ] ) -swarmbot( name "r1121" pose [ 6.000 -14.750 0 0 ] ) -swarmbot( name "r1122" pose [ 6.000 -14.500 0 0 ] ) -swarmbot( name "r1123" pose [ 6.000 -14.250 0 0 ] ) -swarmbot( name "r1124" pose [ 6.000 -14.000 0 0 ] ) -swarmbot( name "r1125" pose [ 6.250 -15.000 0 0 ] ) -swarmbot( name "r1126" pose [ 6.250 -14.750 0 0 ] ) -swarmbot( name "r1127" pose [ 6.250 -14.500 0 0 ] ) -swarmbot( name "r1128" pose [ 6.250 -14.250 0 0 ] ) -swarmbot( name "r1129" pose [ 6.250 -14.000 0 0 ] ) -swarmbot( name "r1130" pose [ 6.500 -15.000 0 0 ] ) -swarmbot( name "r1131" pose [ 6.500 -14.750 0 0 ] ) -swarmbot( name "r1132" pose [ 6.500 -14.500 0 0 ] ) -swarmbot( name "r1133" pose [ 6.500 -14.250 0 0 ] ) -swarmbot( name "r1134" pose [ 6.500 -14.000 0 0 ] ) -swarmbot( name "r1135" pose [ 6.750 -15.000 0 0 ] ) -swarmbot( name "r1136" pose [ 6.750 -14.750 0 0 ] ) -swarmbot( name "r1137" pose [ 6.750 -14.500 0 0 ] ) -swarmbot( name "r1138" pose [ 6.750 -14.250 0 0 ] ) -swarmbot( name "r1139" pose [ 6.750 -14.000 0 0 ] ) -swarmbot( name "r1140" pose [ 7.000 -15.000 0 0 ] ) -swarmbot( name "r1141" pose [ 7.000 -14.750 0 0 ] ) -swarmbot( name "r1142" pose [ 7.000 -14.500 0 0 ] ) -swarmbot( name "r1143" pose [ 7.000 -14.250 0 0 ] ) -swarmbot( name "r1144" pose [ 7.000 -14.000 0 0 ] ) -swarmbot( name "r1145" pose [ 7.250 -15.000 0 0 ] ) -swarmbot( name "r1146" pose [ 7.250 -14.750 0 0 ] ) -swarmbot( name "r1147" pose [ 7.250 -14.500 0 0 ] ) -swarmbot( name "r1148" pose [ 7.250 -14.250 0 0 ] ) -swarmbot( name "r1149" pose [ 7.250 -14.000 0 0 ] ) -swarmbot( name "r1150" pose [ 7.500 -15.000 0 0 ] ) -swarmbot( name "r1151" pose [ 7.500 -14.750 0 0 ] ) -swarmbot( name "r1152" pose [ 7.500 -14.500 0 0 ] ) -swarmbot( name "r1153" pose [ 7.500 -14.250 0 0 ] ) -swarmbot( name "r1154" pose [ 7.500 -14.000 0 0 ] ) -swarmbot( name "r1155" pose [ 7.750 -15.000 0 0 ] ) -swarmbot( name "r1156" pose [ 7.750 -14.750 0 0 ] ) -swarmbot( name "r1157" pose [ 7.750 -14.500 0 0 ] ) -swarmbot( name "r1158" pose [ 7.750 -14.250 0 0 ] ) -swarmbot( name "r1159" pose [ 7.750 -14.000 0 0 ] ) -swarmbot( name "r1160" pose [ 8.000 -15.000 0 0 ] ) -swarmbot( name "r1161" pose [ 8.000 -14.750 0 0 ] ) -swarmbot( name "r1162" pose [ 8.000 -14.500 0 0 ] ) -swarmbot( name "r1163" pose [ 8.000 -14.250 0 0 ] ) -swarmbot( name "r1164" pose [ 8.000 -14.000 0 0 ] ) -swarmbot( name "r1165" pose [ 8.250 -15.000 0 0 ] ) -swarmbot( name "r1166" pose [ 8.250 -14.750 0 0 ] ) -swarmbot( name "r1167" pose [ 8.250 -14.500 0 0 ] ) -swarmbot( name "r1168" pose [ 8.250 -14.250 0 0 ] ) -swarmbot( name "r1169" pose [ 8.250 -14.000 0 0 ] ) -swarmbot( name "r1170" pose [ 8.500 -15.000 0 0 ] ) -swarmbot( name "r1171" pose [ 8.500 -14.750 0 0 ] ) -swarmbot( name "r1172" pose [ 8.500 -14.500 0 0 ] ) -swarmbot( name "r1173" pose [ 8.500 -14.250 0 0 ] ) -swarmbot( name "r1174" pose [ 8.500 -14.000 0 0 ] ) -swarmbot( name "r1175" pose [ 8.750 -15.000 0 0 ] ) -swarmbot( name "r1176" pose [ 8.750 -14.750 0 0 ] ) -swarmbot( name "r1177" pose [ 8.750 -14.500 0 0 ] ) -swarmbot( name "r1178" pose [ 8.750 -14.250 0 0 ] ) -swarmbot( name "r1179" pose [ 8.750 -14.000 0 0 ] ) -swarmbot( name "r1180" pose [ 9.000 -15.000 0 0 ] ) -swarmbot( name "r1181" pose [ 9.000 -14.750 0 0 ] ) -swarmbot( name "r1182" pose [ 9.000 -14.500 0 0 ] ) -swarmbot( name "r1183" pose [ 9.000 -14.250 0 0 ] ) -swarmbot( name "r1184" pose [ 9.000 -14.000 0 0 ] ) -swarmbot( name "r1185" pose [ 9.250 -15.000 0 0 ] ) -swarmbot( name "r1186" pose [ 9.250 -14.750 0 0 ] ) -swarmbot( name "r1187" pose [ 9.250 -14.500 0 0 ] ) -swarmbot( name "r1188" pose [ 9.250 -14.250 0 0 ] ) -swarmbot( name "r1189" pose [ 9.250 -14.000 0 0 ] ) -swarmbot( name "r1190" pose [ 9.500 -15.000 0 0 ] ) -swarmbot( name "r1191" pose [ 9.500 -14.750 0 0 ] ) -swarmbot( name "r1192" pose [ 9.500 -14.500 0 0 ] ) -swarmbot( name "r1193" pose [ 9.500 -14.250 0 0 ] ) -swarmbot( name "r1194" pose [ 9.500 -14.000 0 0 ] ) -swarmbot( name "r1195" pose [ 9.750 -15.000 0 0 ] ) -swarmbot( name "r1196" pose [ 9.750 -14.750 0 0 ] ) -swarmbot( name "r1197" pose [ 9.750 -14.500 0 0 ] ) -swarmbot( name "r1198" pose [ 9.750 -14.250 0 0 ] ) -swarmbot( name "r1199" pose [ 9.750 -14.000 0 0 ] ) -swarmbot( name "r1200" pose [ 10.000 -15.000 0 0 ] ) -swarmbot( name "r1201" pose [ 10.000 -14.750 0 0 ] ) -swarmbot( name "r1202" pose [ 10.000 -14.500 0 0 ] ) -swarmbot( name "r1203" pose [ 10.000 -14.250 0 0 ] ) -swarmbot( name "r1204" pose [ 10.000 -14.000 0 0 ] ) -swarmbot( name "r1205" pose [ 10.250 -15.000 0 0 ] ) -swarmbot( name "r1206" pose [ 10.250 -14.750 0 0 ] ) -swarmbot( name "r1207" pose [ 10.250 -14.500 0 0 ] ) -swarmbot( name "r1208" pose [ 10.250 -14.250 0 0 ] ) -swarmbot( name "r1209" pose [ 10.250 -14.000 0 0 ] ) -swarmbot( name "r1210" pose [ 10.500 -15.000 0 0 ] ) -swarmbot( name "r1211" pose [ 10.500 -14.750 0 0 ] ) -swarmbot( name "r1212" pose [ 10.500 -14.500 0 0 ] ) -swarmbot( name "r1213" pose [ 10.500 -14.250 0 0 ] ) -swarmbot( name "r1214" pose [ 10.500 -14.000 0 0 ] ) -swarmbot( name "r1215" pose [ 10.750 -15.000 0 0 ] ) -swarmbot( name "r1216" pose [ 10.750 -14.750 0 0 ] ) -swarmbot( name "r1217" pose [ 10.750 -14.500 0 0 ] ) -swarmbot( name "r1218" pose [ 10.750 -14.250 0 0 ] ) -swarmbot( name "r1219" pose [ 10.750 -14.000 0 0 ] ) -swarmbot( name "r1220" pose [ 11.000 -15.000 0 0 ] ) -swarmbot( name "r1221" pose [ 11.000 -14.750 0 0 ] ) -swarmbot( name "r1222" pose [ 11.000 -14.500 0 0 ] ) -swarmbot( name "r1223" pose [ 11.000 -14.250 0 0 ] ) -swarmbot( name "r1224" pose [ 11.000 -14.000 0 0 ] ) -swarmbot( name "r1225" pose [ 11.250 -15.000 0 0 ] ) -swarmbot( name "r1226" pose [ 11.250 -14.750 0 0 ] ) -swarmbot( name "r1227" pose [ 11.250 -14.500 0 0 ] ) -swarmbot( name "r1228" pose [ 11.250 -14.250 0 0 ] ) -swarmbot( name "r1229" pose [ 11.250 -14.000 0 0 ] ) -swarmbot( name "r1230" pose [ 11.500 -15.000 0 0 ] ) -swarmbot( name "r1231" pose [ 11.500 -14.750 0 0 ] ) -swarmbot( name "r1232" pose [ 11.500 -14.500 0 0 ] ) -swarmbot( name "r1233" pose [ 11.500 -14.250 0 0 ] ) -swarmbot( name "r1234" pose [ 11.500 -14.000 0 0 ] ) -swarmbot( name "r1235" pose [ 11.750 -15.000 0 0 ] ) -swarmbot( name "r1236" pose [ 11.750 -14.750 0 0 ] ) -swarmbot( name "r1237" pose [ 11.750 -14.500 0 0 ] ) -swarmbot( name "r1238" pose [ 11.750 -14.250 0 0 ] ) -swarmbot( name "r1239" pose [ 11.750 -14.000 0 0 ] ) -swarmbot( name "r1240" pose [ 12.000 -15.000 0 0 ] ) -swarmbot( name "r1241" pose [ 12.000 -14.750 0 0 ] ) -swarmbot( name "r1242" pose [ 12.000 -14.500 0 0 ] ) -swarmbot( name "r1243" pose [ 12.000 -14.250 0 0 ] ) -swarmbot( name "r1244" pose [ 12.000 -14.000 0 0 ] ) -swarmbot( name "r1245" pose [ 12.250 -15.000 0 0 ] ) -swarmbot( name "r1246" pose [ 12.250 -14.750 0 0 ] ) -swarmbot( name "r1247" pose [ 12.250 -14.500 0 0 ] ) -swarmbot( name "r1248" pose [ 12.250 -14.250 0 0 ] ) -swarmbot( name "r1249" pose [ 12.250 -14.000 0 0 ] ) -swarmbot( name "r1250" pose [ 12.500 -15.000 0 0 ] ) -swarmbot( name "r1251" pose [ 12.500 -14.750 0 0 ] ) -swarmbot( name "r1252" pose [ 12.500 -14.500 0 0 ] ) -swarmbot( name "r1253" pose [ 12.500 -14.250 0 0 ] ) -swarmbot( name "r1254" pose [ 12.500 -14.000 0 0 ] ) -swarmbot( name "r1255" pose [ 12.750 -15.000 0 0 ] ) -swarmbot( name "r1256" pose [ 12.750 -14.750 0 0 ] ) -swarmbot( name "r1257" pose [ 12.750 -14.500 0 0 ] ) -swarmbot( name "r1258" pose [ 12.750 -14.250 0 0 ] ) -swarmbot( name "r1259" pose [ 12.750 -14.000 0 0 ] ) -swarmbot( name "r1260" pose [ 13.000 -15.000 0 0 ] ) -swarmbot( name "r1261" pose [ 13.000 -14.750 0 0 ] ) -swarmbot( name "r1262" pose [ 13.000 -14.500 0 0 ] ) -swarmbot( name "r1263" pose [ 13.000 -14.250 0 0 ] ) -swarmbot( name "r1264" pose [ 13.000 -14.000 0 0 ] ) -swarmbot( name "r1265" pose [ 13.250 -15.000 0 0 ] ) -swarmbot( name "r1266" pose [ 13.250 -14.750 0 0 ] ) -swarmbot( name "r1267" pose [ 13.250 -14.500 0 0 ] ) -swarmbot( name "r1268" pose [ 13.250 -14.250 0 0 ] ) -swarmbot( name "r1269" pose [ 13.250 -14.000 0 0 ] ) -swarmbot( name "r1270" pose [ 13.500 -15.000 0 0 ] ) -swarmbot( name "r1271" pose [ 13.500 -14.750 0 0 ] ) -swarmbot( name "r1272" pose [ 13.500 -14.500 0 0 ] ) -swarmbot( name "r1273" pose [ 13.500 -14.250 0 0 ] ) -swarmbot( name "r1274" pose [ 13.500 -14.000 0 0 ] ) -swarmbot( name "r1275" pose [ 13.750 -15.000 0 0 ] ) -swarmbot( name "r1276" pose [ 13.750 -14.750 0 0 ] ) -swarmbot( name "r1277" pose [ 13.750 -14.500 0 0 ] ) -swarmbot( name "r1278" pose [ 13.750 -14.250 0 0 ] ) -swarmbot( name "r1279" pose [ 13.750 -14.000 0 0 ] ) -swarmbot( name "r1280" pose [ 14.000 -15.000 0 0 ] ) -swarmbot( name "r1281" pose [ 14.000 -14.750 0 0 ] ) -swarmbot( name "r1282" pose [ 14.000 -14.500 0 0 ] ) -swarmbot( name "r1283" pose [ 14.000 -14.250 0 0 ] ) -swarmbot( name "r1284" pose [ 14.000 -14.000 0 0 ] ) -swarmbot( name "r1285" pose [ 14.250 -15.000 0 0 ] ) -swarmbot( name "r1286" pose [ 14.250 -14.750 0 0 ] ) -swarmbot( name "r1287" pose [ 14.250 -14.500 0 0 ] ) -swarmbot( name "r1288" pose [ 14.250 -14.250 0 0 ] ) -swarmbot( name "r1289" pose [ 14.250 -14.000 0 0 ] ) -swarmbot( name "r1290" pose [ 14.500 -15.000 0 0 ] ) -swarmbot( name "r1291" pose [ 14.500 -14.750 0 0 ] ) -swarmbot( name "r1292" pose [ 14.500 -14.500 0 0 ] ) -swarmbot( name "r1293" pose [ 14.500 -14.250 0 0 ] ) -swarmbot( name "r1294" pose [ 14.500 -14.000 0 0 ] ) -swarmbot( name "r1295" pose [ 14.750 -15.000 0 0 ] ) -swarmbot( name "r1296" pose [ 14.750 -14.750 0 0 ] ) -swarmbot( name "r1297" pose [ 14.750 -14.500 0 0 ] ) -swarmbot( name "r1298" pose [ 14.750 -14.250 0 0 ] ) -swarmbot( name "r1299" pose [ 14.750 -14.000 0 0 ] ) -swarmbot( name "r1300" pose [ 15.000 -15.000 0 0 ] ) -swarmbot( name "r1301" pose [ 15.000 -14.750 0 0 ] ) -swarmbot( name "r1302" pose [ 15.000 -14.500 0 0 ] ) -swarmbot( name "r1303" pose [ 15.000 -14.250 0 0 ] ) -swarmbot( name "r1304" pose [ 15.000 -14.000 0 0 ] ) -swarmbot( name "r1305" pose [ 15.250 -15.000 0 0 ] ) -swarmbot( name "r1306" pose [ 15.250 -14.750 0 0 ] ) -swarmbot( name "r1307" pose [ 15.250 -14.500 0 0 ] ) -swarmbot( name "r1308" pose [ 15.250 -14.250 0 0 ] ) -swarmbot( name "r1309" pose [ 15.250 -14.000 0 0 ] ) -swarmbot( name "r1310" pose [ 15.500 -15.000 0 0 ] ) -swarmbot( name "r1311" pose [ 15.500 -14.750 0 0 ] ) -swarmbot( name "r1312" pose [ 15.500 -14.500 0 0 ] ) -swarmbot( name "r1313" pose [ 15.500 -14.250 0 0 ] ) -swarmbot( name "r1314" pose [ 15.500 -14.000 0 0 ] ) -swarmbot( name "r1315" pose [ 15.750 -15.000 0 0 ] ) -swarmbot( name "r1316" pose [ 15.750 -14.750 0 0 ] ) -swarmbot( name "r1317" pose [ 15.750 -14.500 0 0 ] ) -swarmbot( name "r1318" pose [ 15.750 -14.250 0 0 ] ) -swarmbot( name "r1319" pose [ 15.750 -14.000 0 0 ] ) -swarmbot( name "r1320" pose [ 16.000 -15.000 0 0 ] ) -swarmbot( name "r1321" pose [ 16.000 -14.750 0 0 ] ) -swarmbot( name "r1322" pose [ 16.000 -14.500 0 0 ] ) -swarmbot( name "r1323" pose [ 16.000 -14.250 0 0 ] ) -swarmbot( name "r1324" pose [ 16.000 -14.000 0 0 ] ) -swarmbot( name "r1325" pose [ 16.250 -15.000 0 0 ] ) -swarmbot( name "r1326" pose [ 16.250 -14.750 0 0 ] ) -swarmbot( name "r1327" pose [ 16.250 -14.500 0 0 ] ) -swarmbot( name "r1328" pose [ 16.250 -14.250 0 0 ] ) -swarmbot( name "r1329" pose [ 16.250 -14.000 0 0 ] ) -swarmbot( name "r1330" pose [ 16.500 -15.000 0 0 ] ) -swarmbot( name "r1331" pose [ 16.500 -14.750 0 0 ] ) -swarmbot( name "r1332" pose [ 16.500 -14.500 0 0 ] ) -swarmbot( name "r1333" pose [ 16.500 -14.250 0 0 ] ) -swarmbot( name "r1334" pose [ 16.500 -14.000 0 0 ] ) -swarmbot( name "r1335" pose [ 16.750 -15.000 0 0 ] ) -swarmbot( name "r1336" pose [ 16.750 -14.750 0 0 ] ) -swarmbot( name "r1337" pose [ 16.750 -14.500 0 0 ] ) -swarmbot( name "r1338" pose [ 16.750 -14.250 0 0 ] ) -swarmbot( name "r1339" pose [ 16.750 -14.000 0 0 ] ) -swarmbot( name "r1340" pose [ 17.000 -15.000 0 0 ] ) -swarmbot( name "r1341" pose [ 17.000 -14.750 0 0 ] ) -swarmbot( name "r1342" pose [ 17.000 -14.500 0 0 ] ) -swarmbot( name "r1343" pose [ 17.000 -14.250 0 0 ] ) -swarmbot( name "r1344" pose [ 17.000 -14.000 0 0 ] ) -swarmbot( name "r1345" pose [ 17.250 -15.000 0 0 ] ) -swarmbot( name "r1346" pose [ 17.250 -14.750 0 0 ] ) -swarmbot( name "r1347" pose [ 17.250 -14.500 0 0 ] ) -swarmbot( name "r1348" pose [ 17.250 -14.250 0 0 ] ) -swarmbot( name "r1349" pose [ 17.250 -14.000 0 0 ] ) -swarmbot( name "r1350" pose [ 17.500 -15.000 0 0 ] ) -swarmbot( name "r1351" pose [ 17.500 -14.750 0 0 ] ) -swarmbot( name "r1352" pose [ 17.500 -14.500 0 0 ] ) -swarmbot( name "r1353" pose [ 17.500 -14.250 0 0 ] ) -swarmbot( name "r1354" pose [ 17.500 -14.000 0 0 ] ) -swarmbot( name "r1355" pose [ 17.750 -15.000 0 0 ] ) -swarmbot( name "r1356" pose [ 17.750 -14.750 0 0 ] ) -swarmbot( name "r1357" pose [ 17.750 -14.500 0 0 ] ) -swarmbot( name "r1358" pose [ 17.750 -14.250 0 0 ] ) -swarmbot( name "r1359" pose [ 17.750 -14.000 0 0 ] ) -swarmbot( name "r1360" pose [ 18.000 -15.000 0 0 ] ) -swarmbot( name "... [truncated message content] |