From: <jer...@us...> - 2008-07-17 14:30:19
|
Revision: 6885 http://playerstage.svn.sourceforge.net/playerstage/?rev=6885&view=rev Author: jeremy_asher Date: 2008-07-17 21:30:23 +0000 (Thu, 17 Jul 2008) Log Message: ----------- stage: added player plugin test, added CMake options to toggle building plugin and plugin tests Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/libstageplugin/CMakeLists.txt code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_fiducial.cc code/stage/trunk/libstageplugin/p_speech.cc code/stage/trunk/worlds/simple.cfg Added Paths: ----------- code/stage/trunk/libstageplugin/test/ code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test.cc Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/CMakeLists.txt 2008-07-17 21:30:23 UTC (rev 6885) @@ -10,6 +10,8 @@ # minimum version of Player to build the plugin SET( MIN_PLAYER 2.1.0 ) +OPTION (BUILD_PLAYER_PLUGIN "Build Player plugin" ON) +OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) cmake_minimum_required( VERSION 2.4 FATAL_ERROR ) @@ -59,7 +61,6 @@ MESSAGE (STATUS "pkg-config not found, skipping Player plugin") ENDIF (PKGCONFIG) - # find FLTK and set flags FIND_PROGRAM (FLTKCONFIG NAMES fltk-config) IF (FLTKCONFIG) @@ -107,9 +108,9 @@ ADD_SUBDIRECTORY(assets) ADD_SUBDIRECTORY(worlds) -IF ( PLAYER_FOUND ) +IF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND ) ADD_SUBDIRECTORY(libstageplugin) -ENDIF ( PLAYER_FOUND ) +ENDIF ( BUILD_PLAYER_PLUGIN AND PLAYER_FOUND ) # generate a cpack config file used to create packaged tarballs Modified: code/stage/trunk/libstageplugin/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-17 21:30:23 UTC (rev 6885) @@ -1,3 +1,4 @@ +MESSAGE( STATUS "Building Player plugin" ) link_directories( ${PLAYER_LIBDIR} ) include_directories( ${PLAYER_INCLUDE_DIRS}) @@ -26,6 +27,10 @@ ) +IF (BUILD_LSPTEST) + ADD_SUBDIRECTORY(test) +ENDIF (BUILD_LSPTEST) + INSTALL(TARGETS stageplugin LIBRARY DESTINATION lib ) Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/libstageplugin/p_driver.cc 2008-07-17 21:30:23 UTC (rev 6885) @@ -568,6 +568,7 @@ for( int i=0; i<(int)this->devices->len; i++ ) { Interface* interface = (Interface*)g_ptr_array_index( this->devices, i ); + assert( interface ); switch( interface->addr.interf ) Modified: code/stage/trunk/libstageplugin/p_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-17 21:30:23 UTC (rev 6885) @@ -68,11 +68,7 @@ assert( fids ); pdata.fiducials_count = fidmod->fiducial_count; - - //printf( "reporting %d fiducials\n", - // fcount ); - - pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count]; + pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count]; for( int i=0; i<(int)pdata.fiducials_count; i++ ) { Modified: code/stage/trunk/libstageplugin/p_speech.cc =================================================================== --- code/stage/trunk/libstageplugin/p_speech.cc 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/libstageplugin/p_speech.cc 2008-07-17 21:30:23 UTC (rev 6885) @@ -81,9 +81,6 @@ mod->Say( pcmd->string ); - this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_SPEECH_CMD_SAY); return( 0 ); } else Added: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt (rev 0) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-17 21:30:23 UTC (rev 6885) @@ -0,0 +1,10 @@ +MESSAGE( STATUS "Building Player plugin tests" ) + +pkg_search_module( PLAYERC REQUIRED playerc ) + +SET( lspTestSrcs + lsp_test.cc +) + +add_executable( lsp_test ${lspTestSrcs} ) +target_link_libraries( lsp_test PLAYERC ) Added: code/stage/trunk/libstageplugin/test/lsp_test.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test.cc 2008-07-17 21:30:23 UTC (rev 6885) @@ -0,0 +1,42 @@ +#include <stdio.h> + +#include <libplayerc/playerc.h> + +int main(int argc, const char **argv) +{ + int i; + playerc_client_t *client; + playerc_position2d_t *position2d; + + // Create a client and connect it to the server. + client = playerc_client_create(NULL, "localhost", 6665); + if (0 != playerc_client_connect(client)) + return -1; + + // Create and subscribe to a position2d device. + position2d = playerc_position2d_create(client, 0); + if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE)) + return -1; + + // Make the robot spin! + if (0 != playerc_position2d_set_cmd_vel(position2d, 0, 0, DTOR(40.0), 1)) + return -1; + + for (i = 0; i < 200; i++) + { + // Wait for new data from server + playerc_client_read(client); + + // Print current robot pose + printf("position2d : %f %f %f\n", + position2d->px, position2d->py, position2d->pa); + } + + // Shutdown + playerc_position2d_unsubscribe(position2d); + playerc_position2d_destroy(position2d); + playerc_client_disconnect(client); + playerc_client_destroy(client); + + return 0; +} Modified: code/stage/trunk/worlds/simple.cfg =================================================================== --- code/stage/trunk/worlds/simple.cfg 2008-07-17 21:22:33 UTC (rev 6884) +++ code/stage/trunk/worlds/simple.cfg 2008-07-17 21:30:23 UTC (rev 6885) @@ -18,7 +18,7 @@ ) # Create a Stage driver and attach position2d and laser interfaces -# to the model "robot1" +# to the model "r0" driver ( name "stage" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-18 11:18:08
|
Revision: 6888 http://playerstage.svn.sourceforge.net/playerstage/?rev=6888&view=rev Author: jeremy_asher Date: 2008-07-18 18:18:17 +0000 (Fri, 18 Jul 2008) Log Message: ----------- stage: applied gbiggs's patch to fix '[ 2021110 ] Stage 3 doesn't use fltk-config output', several other build tweaks Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/worlds/benchmark/CMakeLists.txt Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-18 17:42:14 UTC (rev 6887) +++ code/stage/trunk/CMakeLists.txt 2008-07-18 18:18:17 UTC (rev 6888) @@ -68,7 +68,7 @@ OUTPUT_VARIABLE FLTK_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) EXECUTE_PROCESS (COMMAND fltk-config --ldflags --use-gl --use-images - OUTPUT_VARIABLE FLTK_LIBRARIES + OUTPUT_VARIABLE FLTK_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) MESSAGE (STATUS "Found FLTK") SET (FLTK_FOUND TRUE) @@ -89,9 +89,9 @@ include_directories( . libstage replace - /opt/local/include/ - ${GLIB_INCLUDE_DIRS} - ${LIBPNG_INCLUDE_DIRS} + /opt/local/include/ + ${GLIB_INCLUDE_DIRS} + ${LIBPNG_INCLUDE_DIRS} ${CMAKE_INCLUDE_PATH} ) @@ -99,7 +99,7 @@ # all targets need these library directories link_directories(${GLIB_LIBRARY_DIRS} ${LIBPNG_LIBRARY_DIRS} - ${OPENGL_LIBRARY_DIRS} + ${OPENGL_LIBRARY_DIRS} ) # work through these subdirs Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2008-07-18 17:42:14 UTC (rev 6887) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2008-07-18 18:18:17 UTC (rev 6888) @@ -1,9 +1,9 @@ -SET( PLUGINS - fasr - lasernoise - sink - source +SET( PLUGINS + fasr + lasernoise + sink + source wander ) Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-07-18 17:42:14 UTC (rev 6887) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-07-18 18:18:17 UTC (rev 6888) @@ -1,46 +1,52 @@ - -add_library(stage SHARED - ancestor.cc - block.cc +set( stageSrcs ancestor.cc + block.cc camera.cc - canvas.cc + canvas.cc file_manager.cc file_manager.hh - gl.cc - glcolorstack.cc - model.cc + gl.cc + glcolorstack.cc + model.cc model_blinkenlight.cc - model_blobfinder.cc - model_callbacks.cc - model_camera.cc - model_fiducial.cc - model_laser.cc - model_load.cc - model_position.cc - model_props.cc - model_ranger.cc + model_blobfinder.cc + model_callbacks.cc + model_camera.cc + model_fiducial.cc + model_laser.cc + model_load.cc + model_position.cc + model_props.cc + model_ranger.cc option.cc option.hh options_dlg.cc options_dlg.hh resource.cc - stage.cc + stage.cc stage.hh texture_manager.cc - typetable.cc - world.cc - worldfile.cc + typetable.cc + world.cc + worldfile.cc worldgui.cc ) +add_library(stage SHARED ${stageSrcs}) + target_link_libraries( stage ${GLIB_LIBRARIES} ${OPENGL_LIBRARIES} - ${FLTK_LIBRARIES} ${APPLE_LIBRARIES} ltdl ) +# When linking stage, pass LDFLAGS from fltk-config +# use quotes to prevent spaces being parsed as a list of properties +set_target_properties( stage PROPERTIES LINK_FLAGS "${FLTK_LDFLAGS}" ) + +# When compiling stageSrcs, pass CFLAGS from fltk-config +set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) + # for the config.h include_directories(${PROJECT_BINARY_DIR}) @@ -49,26 +55,27 @@ VERSION ${VERSION} ) +set( stagebinarySrcs main.cc ) -add_executable( stagebinary main.cc ) +add_executable( stagebinary ${stagebinarySrcs} ) +# When compiling stagebinarySrcs, pass CFLAGS from fltk-config +set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) + set_target_properties( stagebinary PROPERTIES - OUTPUT_NAME stage + OUTPUT_NAME stage ) +target_link_libraries( stagebinary stage ) -set_target_properties( stagebinary PROPERTIES - OUTPUT_NAME stage -) +set( stagetestSrcs test.cc ) -target_link_libraries( stagebinary stage ) - -add_executable( stagetest test.cc ) +add_executable( stagetest ${stagetestSrcs} ) target_link_libraries( stagetest stage ) INSTALL(TARGETS stagebinary stage - RUNTIME DESTINATION bin + RUNTIME DESTINATION bin LIBRARY DESTINATION lib ) Modified: code/stage/trunk/worlds/benchmark/CMakeLists.txt =================================================================== --- code/stage/trunk/worlds/benchmark/CMakeLists.txt 2008-07-18 17:42:14 UTC (rev 6887) +++ code/stage/trunk/worlds/benchmark/CMakeLists.txt 2008-07-18 18:18:17 UTC (rev 6888) @@ -1,4 +1,5 @@ -ADD_LIBRARY( expand MODULE expand.cc ) +SET( expandSrcs expand.cc ) +ADD_LIBRARY( expand MODULE ${expandSrcs} ) TARGET_LINK_LIBRARIES( expand stage ) SET_TARGET_PROPERTIES( expand PROPERTIES PREFIX "" ) INSTALL( TARGETS expand DESTINATION lib) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-21 16:39:14
|
Revision: 6899 http://playerstage.svn.sourceforge.net/playerstage/?rev=6899&view=rev Author: jeremy_asher Date: 2008-07-21 23:39:22 +0000 (Mon, 21 Jul 2008) Log Message: ----------- libstageplugin: fixed a few bugs with laser interface configuration setting, speech interface should be working, updated tests Modified Paths: -------------- code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/libstageplugin/p_speech.cc code/stage/trunk/libstageplugin/test/lsp_test_laser.cc code/stage/trunk/libstageplugin/test/lsp_test_laser.hh Added Paths: ----------- code/stage/trunk/worlds/lsp_test.cfg code/stage/trunk/worlds/lsp_test.world Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstage/model_laser.cc 2008-07-21 23:39:22 UTC (rev 6899) @@ -141,6 +141,7 @@ cfg.range_bounds.max = range_max; cfg.fov = fov; cfg.resolution = resolution; + cfg.interval = interval; return cfg; } @@ -150,6 +151,7 @@ range_max = cfg.range_bounds.max; fov = cfg.fov; resolution = cfg.resolution; + interval = cfg.interval; } static bool laser_raytrace_match( StgBlock* testblock, Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstage/stage.hh 2008-07-21 23:39:22 UTC (rev 6899) @@ -1196,8 +1196,8 @@ int subs; //< the number of subscriptions to this model - stg_usec_t interval; //< time between updates in ms - stg_usec_t last_update; //< time of last update in ms + stg_usec_t interval; //< time between updates in us + stg_usec_t last_update; //< time of last update in us stg_bool_t disabled; //< if non-zero, the model is disabled @@ -2162,6 +2162,7 @@ uint32_t resolution; stg_range_bounds_t range_bounds; stg_radians_t fov; + stg_usec_t interval; } stg_laser_cfg_t; /// %StgModelLaser class Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-21 23:39:22 UTC (rev 6899) @@ -62,11 +62,10 @@ memset( &pdata, 0, sizeof(pdata) ); stg_laser_cfg_t cfg = mod->GetConfig(); - pdata.min_angle = -cfg.fov/2.0; pdata.max_angle = +cfg.fov/2.0; + pdata.resolution = cfg.fov / cfg.sample_count; pdata.max_range = cfg.range_bounds.max; - pdata.resolution = cfg.fov / (double)(cfg.sample_count-1); pdata.ranges_count = pdata.intensity_count = cfg.sample_count; pdata.id = this->scan_id++; @@ -114,18 +113,22 @@ plc->resolution/1e2); stg_laser_cfg_t cfg = mod->GetConfig(); + + cfg.resolution = 1.0 / plc->resolution; + cfg.range_bounds.max = plc->max_range; + cfg.fov = plc->max_angle - plc->min_angle; + cfg.interval = 1E6 / plc->scanning_frequency; - // tweak the parts that player knows about - cfg.fov = plc->max_angle - plc->min_angle; + PRINT_DEBUG4( "setting laser config: fov %.2f max_range %.2f " / + "resolution %.2f interval %.2f", + cfg.fov, cfg.range_bounds.max, + cfg.resolution, cfg.interval ); - // todo - test this - cfg.sample_count = (int)(cfg.fov / DTOR(plc->resolution/1e2)); - - PRINT_DEBUG2( "setting laser config: fov %.2f samples %d", - slc.fov, slc.samples ); - + // Range resolution is currently locked to the world setting + // and intensity values are always read. The relevant settings + // are ignored. + mod->SetConfig( cfg ); - this->driver->Publish(this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, @@ -153,9 +156,11 @@ memset(&plc,0,sizeof(plc)); plc.min_angle = -cfg.fov/2.0; plc.max_angle = +cfg.fov/2.0; + plc.resolution = cfg.fov / cfg.sample_count; plc.max_range = cfg.range_bounds.max; - plc.resolution = (uint8_t)(RTOD(cfg.fov / (cfg.sample_count-1)) * 100); + plc.range_res = 1.0; // todo plc.intensity = 1; // todo + plc.scanning_frequency = 1E6 / cfg.interval; this->driver->Publish(this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, Modified: code/stage/trunk/libstageplugin/p_speech.cc =================================================================== --- code/stage/trunk/libstageplugin/p_speech.cc 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstageplugin/p_speech.cc 2008-07-21 23:39:22 UTC (rev 6899) @@ -73,8 +73,8 @@ PLAYER_SPEECH_CMD_SAY, this->addr)) { - if( hdr->size == sizeof(player_speech_cmd_t) ) - { +// if( hdr->size == sizeof(player_speech_cmd_t) ) +// { player_speech_cmd_t* pcmd = (player_speech_cmd_t*)data; // Pass it to stage: @@ -82,14 +82,14 @@ mod->Say( pcmd->string ); return( 0 ); - } - else - { - PRINT_ERR2( "wrong size speech command packet (%d/%d bytes)", - (int)hdr->size, (int)sizeof(player_speech_cmd_t) ); - - return( -1 ); - } +// } +// else +// { +// PRINT_ERR2( "wrong size speech command packet (%d/%d bytes)", +// (int)hdr->size, (int)sizeof(player_speech_cmd_t) ); +// +// return( -1 ); +// } } PRINT_WARN2( "stage speech doesn't support message id:%d/%d", Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-21 23:39:22 UTC (rev 6899) @@ -2,6 +2,8 @@ using namespace lspTest; +const double Laser::Delta = 0.01; + void Laser::setUp() { connect(); laserProxy = playerc_laser_create( client, 0 ); @@ -16,14 +18,12 @@ } void Laser::testConfig() { - const double DELTA = 0.01; - double min = -M_PI/2; double max = +M_PI/2; - double res = 100; - double range_res = 1; - unsigned char intensity = 1; - double freq = 10; + double res = M_PI/361; // sick laser default + double range_res = 1; // not being used by stage + unsigned char intensity = 1; // not being used by stage + double freq = 1.0/1E4; // stage default CPPUNIT_ASSERT( playerc_laser_set_config( laserProxy, min, max, res, range_res, intensity, freq ) == 0 ); @@ -31,14 +31,34 @@ unsigned char intensity2 = 1; CPPUNIT_ASSERT( playerc_laser_get_config( laserProxy, &min2, &max2, &res2, &range_res2, &intensity2, &freq2 ) == 0 ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2, DELTA ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2, DELTA ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2, DELTA ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res, range_res2, DELTA ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, DELTA ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res, range_res2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, Delta ); CPPUNIT_ASSERT_EQUAL_MESSAGE( "intensity", intensity, intensity2 ); } +void Laser::testGeom() { + CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 ); + + // values for lsp_test.cfg/world (sicklaser) + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, laserProxy->pose[0], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, laserProxy->pose[2], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156, laserProxy->size[0], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155, laserProxy->size[1], Delta ); +} + void Laser::testData() { + playerc_client_read( client ); + printf("\nlaser: [%14.3f ] [%d]\n ", laserProxy->info.datatime, laserProxy->scan_count); + for ( int i = 0; i < laserProxy->scan_count; i++ ) { + printf("[%6.3f, %6.3f ] \n", laserProxy->scan[i][0], laserProxy->scan[i][1]); + } + + // check first and last sample's angle + // check number of samples + // check range of each is within max and min } \ No newline at end of file Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-21 23:21:42 UTC (rev 6898) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-21 23:39:22 UTC (rev 6899) @@ -11,6 +11,7 @@ { CPPUNIT_TEST_SUITE( Laser ); CPPUNIT_TEST( testConfig ); + CPPUNIT_TEST( testGeom ); CPPUNIT_TEST( testData ); CPPUNIT_TEST_SUITE_END(); @@ -18,8 +19,10 @@ playerc_laser_t* laserProxy; void testConfig(); + void testGeom(); void testData(); + static const double Delta; public: void setUp(); void tearDown(); Added: code/stage/trunk/worlds/lsp_test.cfg =================================================================== --- code/stage/trunk/worlds/lsp_test.cfg (rev 0) +++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-21 23:39:22 UTC (rev 6899) @@ -0,0 +1,22 @@ +# Desc: Player configuration file for use with lsp_test utility + +# load the Stage plugin simulation driver +driver +( + name "stage" + provides [ "simulation:0" ] + plugin "libstageplugin" + + # load the named file into the simulator + worldfile "lsp_test.world" +) + +# Create a Stage driver and attach position2d and laser interfaces +# to the model "r0" +driver +( + name "stage" + provides [ "position2d:0" "laser:0" "speech:0" ] + model "r0" +) + Added: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world (rev 0) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-21 23:39:22 UTC (rev 6899) @@ -0,0 +1,37 @@ +# lsp_test.world - test world for use with lsp_test + + +include "pioneer.inc" # defines Pioneer-like robots +include "map.inc" # defines 'floorplan' object used for obstacle maps +include "sick.inc" # defines sick laser + +interval_sim 100 # simulation timestep in milliseconds +interval_real 1 # real-time interval between simulation updates in milliseconds + +paused 0 + +# configure the GUI window +window +( + size [ 700.000 700.000 ] # in pixels + scale 37 # pixels per meter + show_data 1 # 1=on 0=off +) + +# load an environment bitmap +floorplan +( + name "cave" + size [16 16 0.6] + bitmap "bitmaps/cave.png" +) + +pioneer2dx +( + # can refer to the robot by this name + name "r0" + + pose [ -6.5 -6.5 0 45 ] + sicklaser() +) + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-22 17:03:25
|
Revision: 6904 http://playerstage.svn.sourceforge.net/playerstage/?rev=6904&view=rev Author: jeremy_asher Date: 2008-07-23 00:03:34 +0000 (Wed, 23 Jul 2008) Log Message: ----------- libstageplugin: p_position updates, added position2d test Modified Paths: -------------- code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/libstageplugin/p_position.cc code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test_laser.cc code/stage/trunk/libstageplugin/test/lsp_test_laser.hh code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh code/stage/trunk/libstageplugin/test/lsp_test_speech.cc code/stage/trunk/worlds/lsp_test.world Added Paths: ----------- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -110,7 +110,7 @@ stg_laser_cfg_t cfg = mod->GetConfig(); - printf( "laser config was: resolution %d, fov %.6f, interval %d\n", + PRINT_DEBUG3( "laser config was: resolution %d, fov %.6f, interval %d\n", cfg.resolution, cfg.fov, cfg.interval ); cfg.fov = plc->max_angle - plc->min_angle; @@ -119,8 +119,7 @@ cfg.resolution = 1; cfg.interval = 1.0E6 / plc->scanning_frequency; - //PRINT_DEBUG3 - printf( "setting laser config: resolution %d, fov %.6f, interval %d\n", + PRINT_DEBUG3( "setting laser config: resolution %d, fov %.6f, interval %d\n", cfg.resolution, cfg.fov, cfg.interval ); // Range resolution is currently locked to the world setting Modified: code/stage/trunk/libstageplugin/p_position.cc =================================================================== --- code/stage/trunk/libstageplugin/p_position.cc 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/p_position.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -86,7 +86,7 @@ // convert from Player to Stage format player_position2d_cmd_pos_t* pcmd = (player_position2d_cmd_pos_t*)data; - mod->GoTo( pcmd->vel.px, pcmd->vel.py, pcmd->vel.pa ); + mod->GoTo( pcmd->pos.px, pcmd->pos.py, pcmd->pos.pa ); return 0; } @@ -140,12 +140,12 @@ { if(hdr->size == 0) { - PRINT_WARN( "reset odom not implemented" ); PRINT_DEBUG( "resetting odometry" ); - stg_pose_t origin; - memset(&origin,0,sizeof(origin)); - //stg_model_position_set_odom( this->mod, &origin ); + mod->est_pose.x = 0; + mod->est_pose.y = 0; + mod->est_pose.z = 0; + mod->est_pose.a = 0; this->driver->Publish( this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, @@ -166,18 +166,15 @@ { if(hdr->size == sizeof(player_position2d_set_odom_req_t)) { - PRINT_WARN( "set odom not implemented" ); - player_position2d_set_odom_req_t* req = (player_position2d_set_odom_req_t*)data; - stg_pose_t pose; - pose.x = req->pose.px; - pose.y = req->pose.py; - pose.a = req->pose.pa; + + mod->est_pose.x = req->pose.px; + mod->est_pose.y = req->pose.py; + //mod->est_pose.z = req->pose.pz; + mod->est_pose.a = req->pose.pa; - //stg_model_position_set_odom( this->mod, &pose ); - PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)", pose.x, pose.y, Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 00:03:34 UTC (rev 6904) @@ -19,6 +19,8 @@ lsp_test_laser.hh lsp_test_speech.cc lsp_test_speech.hh + lsp_test_position2d.cc + lsp_test_position2d.hh ) add_executable( lsp_test ${lspTestSrcs} ) Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -2,7 +2,6 @@ using namespace lspTest; -const double Laser::Delta = 0.01; const int Laser::Samples = 361; void Laser::setUp() { @@ -46,7 +45,7 @@ // values for lsp_test.cfg/world (sicklaser) CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, laserProxy->pose[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, laserProxy->pose[2], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, laserProxy->pose[2], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156, laserProxy->size[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155, laserProxy->size[1], Delta ); } Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-23 00:03:34 UTC (rev 6904) @@ -22,7 +22,6 @@ void testGeom(); void testData(); - static const double Delta; static const int Samples; public: void setUp(); Added: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -0,0 +1,114 @@ +#include "lsp_test_position2d.hh" + + +using namespace lspTest; + +void Position2D::setUp() { + connect(); + posProxy = playerc_position2d_create( client, 0 ); + CPPUNIT_ASSERT( playerc_position2d_subscribe( posProxy, PLAYER_OPEN_MODE ) == 0 ); + + // ignore initial messages on proxy subscription + for ( int i = 0; i < 10; i++ ) + playerc_client_read( client ); +} + +void Position2D::tearDown() { + CPPUNIT_ASSERT( playerc_position2d_unsubscribe( posProxy ) == 0 ); + playerc_position2d_destroy( posProxy ); + disconnect(); +} + +void Position2D::testGeom() { + CPPUNIT_ASSERT( playerc_position2d_get_geom( posProxy ) == 0 ); + + // values for lsp_test.cfg/world (pioneer2dx) + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (x)", -0.04, posProxy->pose[0], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (y)", 0, posProxy->pose[1], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (angle)", 0, posProxy->pose[2], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom size (x)", 0.44, posProxy->size[0], Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom size (y)", 0.38, posProxy->size[1], Delta ); +} + +void Position2D::testData() { + playerc_client_read( client ); + + // verify that we're getting new data + posProxy->info.fresh = 0; + playerc_client_read( client ); + CPPUNIT_ASSERT_MESSAGE( "laser updating", posProxy->info.fresh == 1 ); + + CPPUNIT_ASSERT( posProxy->info.datatime > 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa, Delta ); + CPPUNIT_ASSERT( posProxy->stall == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0, posProxy->vx, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0, posProxy->vy, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, Delta ); +} + +void Position2D::testMove() { + // Back the robot into the wall + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, -0.5, 0, 0, 1 ) == 0 ); + // for 2 seconds, read and stop + sleep( 2 ); + playerc_client_read( client ); + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); + + // Make sure it moved back + CPPUNIT_ASSERT( posProxy->px < 0 ); + // Make sure it didn't turn or move sideways + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa, Delta ); + // Make sure it's hitting the wall + CPPUNIT_ASSERT( posProxy->stall == 1 ); + // Make sure the velocity is as specified + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", -0.5, posProxy->vx, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0, posProxy->vy, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, Delta ); + + // Start turning left at 45 deg. / sec + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, M_PI/4, 1 ) == 0 ); + // for 1 second, stop, and read + sleep( 1 ); + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); + sleep( 1 ); + playerc_client_read( client ); + + // Make sure odom still reads negative + CPPUNIT_ASSERT( posProxy->px < 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); + // See if we turned in the right direction + CPPUNIT_ASSERT( posProxy > 0 ); + // Shouldn't be stalled + CPPUNIT_ASSERT_EQUAL( 0, posProxy->stall ); + // Should have stopped + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0, posProxy->vx, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0, posProxy->vy, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, Delta ); + + // Save position and try position based movement toward start position + double oldx = posProxy->px; + double olda = posProxy->pa; + CPPUNIT_ASSERT( playerc_position2d_set_cmd_pose( posProxy, 0, 0, 0, 1 ) == 0 ); + sleep( 5 ); + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); + sleep( 1 ); + playerc_client_read( client ); + + // Make sure we've made progress + CPPUNIT_ASSERT( posProxy->px > oldx ); + CPPUNIT_ASSERT( posProxy->pa < olda ); + CPPUNIT_ASSERT( posProxy->stall == 0 ); + // Verify that we've stopped + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0, posProxy->vx, 0.1 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0, posProxy->vy, 0.1 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, 0.1 ); + + // Reset odometer and check position + CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa, Delta ); +} \ No newline at end of file Added: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-23 00:03:34 UTC (rev 6904) @@ -0,0 +1,33 @@ +#ifndef _LSP_POSITION2D_TEST_H_ +#define _LSP_POSITION2D_TEST_H_ + +#include <cppunit/extensions/HelperMacros.h> +#include <libplayerc/playerc.h> + +#include "lsp_test_proxy.hh" + +namespace lspTest { + class Position2D : public Proxy + { + CPPUNIT_TEST_SUB_SUITE( Position2D, Proxy ); + CPPUNIT_TEST( testGeom ); + CPPUNIT_TEST( testData ); + CPPUNIT_TEST( testMove ); + CPPUNIT_TEST_SUITE_END(); + + protected: + playerc_position2d_t* posProxy; + + void testGeom(); + void testData(); + void testMove(); + + public: + void setUp(); + void tearDown(); + }; +}; + +CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Position2D ); + +#endif \ No newline at end of file Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -1,12 +1,14 @@ #include "lsp_test_proxy.hh" -#include <stdio.h> - using namespace lspTest; +const double Proxy::Delta = 0.01; + void Proxy::connect() { client = playerc_client_create( NULL, "localhost", 6665 ); CPPUNIT_ASSERT( playerc_client_connect( client ) == 0 ); + CPPUNIT_ASSERT( playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) == 0 ); + CPPUNIT_ASSERT( playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) == 0 ); } void Proxy::disconnect() { Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-23 00:03:34 UTC (rev 6904) @@ -15,6 +15,8 @@ void connect(); void disconnect(); + + static const double Delta; }; }; Modified: code/stage/trunk/libstageplugin/test/lsp_test_speech.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_speech.cc 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/libstageplugin/test/lsp_test_speech.cc 2008-07-23 00:03:34 UTC (rev 6904) @@ -1,7 +1,5 @@ #include "lsp_test_speech.hh" -#include <stdio.h> - using namespace lspTest; void Speech::setUp() { Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-22 20:23:21 UTC (rev 6903) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-23 00:03:34 UTC (rev 6904) @@ -1,9 +1,9 @@ # lsp_test.world - test world for use with lsp_test -include "pioneer.inc" # defines Pioneer-like robots -include "map.inc" # defines 'floorplan' object used for obstacle maps -include "sick.inc" # defines sick laser +include "pioneer.inc" +include "map.inc" +include "sick.inc" interval_sim 100 # simulation timestep in milliseconds interval_real 1 # real-time interval between simulation updates in milliseconds @@ -14,7 +14,7 @@ window ( size [ 700.000 700.000 ] # in pixels - scale 37 # pixels per meter + scale 37.000 # pixels per meter show_data 1 # 1=on 0=off ) @@ -22,7 +22,7 @@ floorplan ( name "cave" - size [16 16 0.6] + size [16.000 16.000 0.600] bitmap "bitmaps/cave.png" ) @@ -31,7 +31,7 @@ # can refer to the robot by this name name "r0" - pose [ -6.5 -6.5 0 45 ] + pose [ -7.490 -7.490 0 45.000 ] sicklaser() ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-23 14:03:00
|
Revision: 6916 http://playerstage.svn.sourceforge.net/playerstage/?rev=6916&view=rev Author: jeremy_asher Date: 2008-07-23 21:03:09 +0000 (Wed, 23 Jul 2008) Log Message: ----------- stage: fixed odometer updating, libstageplugin: updated position2d test Modified Paths: -------------- code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc code/stage/trunk/worlds/lsp_test.world Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-07-23 16:52:14 UTC (rev 6915) +++ code/stage/trunk/libstage/model_position.cc 2008-07-23 21:03:09 UTC (rev 6916) @@ -411,7 +411,7 @@ case STG_POSITION_LOCALIZATION_ODOM: { // integrate our velocities to get an 'odometry' position estimate. - double dt = this->world->GetSimInterval()/1e3; + double dt = this->world->GetSimInterval()/1e6; est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) ); @@ -432,6 +432,8 @@ break; } + PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n", + est_pose.x, est_pose.y, est_pose.a ); StgModel::Update(); } @@ -497,7 +499,10 @@ void StgModelPosition::SetSpeed( stg_velocity_t vel ) { control_mode = STG_POSITION_CONTROL_VELOCITY; - velocity = vel; + goal.x = vel.x; + goal.y = vel.y; + goal.z = vel.z; + goal.a = vel.a; } void StgModelPosition::GoTo( double x, double y, double a ) Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23 16:52:14 UTC (rev 6915) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23 21:03:09 UTC (rev 6916) @@ -107,7 +107,10 @@ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, 0.1 ); // Reset odometer and check position + playerc_client_read( client ); CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 ); + sleep( 1 ); + playerc_client_read( client ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa, Delta ); Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-23 16:52:14 UTC (rev 6915) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-23 21:03:09 UTC (rev 6916) @@ -31,6 +31,7 @@ # can refer to the robot by this name name "r0" + localization "odom" pose [ -7.490 -7.490 0 45.000 ] sicklaser() ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-23 14:49:54
|
Revision: 6917 http://playerstage.svn.sourceforge.net/playerstage/?rev=6917&view=rev Author: jeremy_asher Date: 2008-07-23 21:50:03 +0000 (Wed, 23 Jul 2008) Log Message: ----------- libstageplugin: added sonar test Modified Paths: -------------- code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test_laser.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc code/stage/trunk/worlds/lsp_test.cfg Added Paths: ----------- code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 21:03:09 UTC (rev 6916) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 21:50:03 UTC (rev 6917) @@ -19,6 +19,8 @@ lsp_test_laser.hh lsp_test_speech.cc lsp_test_speech.hh + lsp_test_sonar.cc + lsp_test_sonar.hh lsp_test_position2d.cc lsp_test_position2d.hh ) Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23 21:03:09 UTC (rev 6916) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23 21:50:03 UTC (rev 6917) @@ -42,7 +42,7 @@ void Laser::testGeom() { CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 ); - // values for lsp_test.cfg/world (sicklaser) + // values from lsp_test.cfg/world and sick.inc CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, laserProxy->pose[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, laserProxy->pose[2], Delta ); Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23 21:03:09 UTC (rev 6916) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23 21:50:03 UTC (rev 6917) @@ -22,7 +22,7 @@ void Position2D::testGeom() { CPPUNIT_ASSERT( playerc_position2d_get_geom( posProxy ) == 0 ); - // values for lsp_test.cfg/world (pioneer2dx) + // values from lsp_test.cfg/world (pioneer2dx) CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (x)", -0.04, posProxy->pose[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (y)", 0, posProxy->pose[1], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (angle)", 0, posProxy->pose[2], Delta ); @@ -50,9 +50,9 @@ void Position2D::testMove() { // Back the robot into the wall - CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, -0.5, 0, 0, 1 ) == 0 ); - // for 2 seconds, read and stop - sleep( 2 ); + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, -3, 0, 0, 1 ) == 0 ); + // for 500ms, read and stop + usleep( 500000 ); playerc_client_read( client ); CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); @@ -64,16 +64,16 @@ // Make sure it's hitting the wall CPPUNIT_ASSERT( posProxy->stall == 1 ); // Make sure the velocity is as specified - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", -0.5, posProxy->vx, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", -3, posProxy->vx, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0, posProxy->vy, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0, posProxy->va, Delta ); // Start turning left at 45 deg. / sec - CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, M_PI/4, 1 ) == 0 ); - // for 1 second, stop, and read - sleep( 1 ); + CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, M_PI/2, 1 ) == 0 ); + // for 500ms, stop, and read + usleep( 500000 ); CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); - sleep( 1 ); + usleep( 500000 ); playerc_client_read( client ); // Make sure odom still reads negative @@ -94,7 +94,7 @@ CPPUNIT_ASSERT( playerc_position2d_set_cmd_pose( posProxy, 0, 0, 0, 1 ) == 0 ); sleep( 5 ); CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 ) == 0 ); - sleep( 1 ); + usleep( 500000 ); playerc_client_read( client ); // Make sure we've made progress @@ -109,7 +109,7 @@ // Reset odometer and check position playerc_client_read( client ); CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 ); - sleep( 1 ); + usleep( 500000 ); playerc_client_read( client ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); Added: code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-23 21:50:03 UTC (rev 6917) @@ -0,0 +1,47 @@ +#include "lsp_test_sonar.hh" + +using namespace lspTest; + +const int Sonar::Samples = 361; + +void Sonar::setUp() { + connect(); + sonarProxy = playerc_sonar_create( client, 0 ); + CPPUNIT_ASSERT( playerc_sonar_subscribe( sonarProxy, PLAYER_OPEN_MODE ) == 0 ); +} + + +void Sonar::tearDown() { + CPPUNIT_ASSERT( playerc_sonar_unsubscribe( sonarProxy ) == 0 ); + playerc_sonar_destroy( sonarProxy ); + disconnect(); +} + +void Sonar::testGeom() { + CPPUNIT_ASSERT( playerc_sonar_get_geom( sonarProxy ) == 0 ); + + // values from pioneer.inc + CPPUNIT_ASSERT( sonarProxy->pose_count == 16 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (x)", 0.075, sonarProxy->poses[0].px, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (y)", 0.130, sonarProxy->poses[0].py, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (z)", 0, sonarProxy->poses[0].pz, Delta ); // currently unused + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (angle)", M_PI/2, sonarProxy->poses[0].pyaw, Delta ); +} + +void Sonar::testData() { + playerc_client_read( client ); + + // verify that we're getting new data + sonarProxy->info.fresh = 0; + playerc_client_read( client ); + CPPUNIT_ASSERT_MESSAGE( "sonar updating", sonarProxy->info.fresh == 1 ); + + CPPUNIT_ASSERT( sonarProxy->info.datatime > 0 ); + CPPUNIT_ASSERT( sonarProxy->scan_count == 16 ); // from pioneer.inc, actually sensor_count + + // sanity check + for ( int i = 0; i < sonarProxy->scan_count; i++ ) { + CPPUNIT_ASSERT( sonarProxy->scan[i] >= 0 ); +// printf(" range[%d]: %6.3f ]\n", i, sonarProxy->scan[i]); + } +} \ No newline at end of file Added: code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-23 21:50:03 UTC (rev 6917) @@ -0,0 +1,33 @@ +#ifndef _LSP_SONAR_TEST_H_ +#define _LSP_SONAR_TEST_H_ + +#include <cppunit/extensions/HelperMacros.h> +#include <libplayerc/playerc.h> + +#include "lsp_test_proxy.hh" + +namespace lspTest { + class Sonar : public Proxy + { + CPPUNIT_TEST_SUITE( Sonar ); + CPPUNIT_TEST( testGeom ); + CPPUNIT_TEST( testData ); + CPPUNIT_TEST_SUITE_END(); + + protected: + playerc_sonar_t* sonarProxy; + + void testConfig(); + void testGeom(); + void testData(); + + static const int Samples; + public: + void setUp(); + void tearDown(); + }; +}; + +CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Sonar ); + +#endif \ No newline at end of file Modified: code/stage/trunk/worlds/lsp_test.cfg =================================================================== --- code/stage/trunk/worlds/lsp_test.cfg 2008-07-23 21:03:09 UTC (rev 6916) +++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-23 21:50:03 UTC (rev 6917) @@ -16,7 +16,7 @@ driver ( name "stage" - provides [ "position2d:0" "laser:0" "speech:0" ] + provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" ] model "r0" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-23 17:03:38
|
Revision: 6919 http://playerstage.svn.sourceforge.net/playerstage/?rev=6919&view=rev Author: jeremy_asher Date: 2008-07-24 00:03:46 +0000 (Thu, 24 Jul 2008) Log Message: ----------- libstageplugin: added fiducial test, updated other tests, fixed initialization of sonar data in p_sonar Modified Paths: -------------- code/stage/trunk/libstageplugin/p_sonar.cc code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test_laser.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh code/stage/trunk/worlds/lsp_test.cfg code/stage/trunk/worlds/lsp_test.world Added Paths: ----------- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh Modified: code/stage/trunk/libstageplugin/p_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/p_sonar.cc 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/libstageplugin/p_sonar.cc 2008-07-24 00:03:46 UTC (rev 6919) @@ -119,6 +119,9 @@ // fill in the geometry data formatted player-like pgeom.poses[i].px = mod->sensors[i].pose.x; pgeom.poses[i].py = mod->sensors[i].pose.y; + pgeom.poses[i].pz = 0; + pgeom.poses[i].ppitch = 0; + pgeom.poses[i].proll = 0; pgeom.poses[i].pyaw = mod->sensors[i].pose.a; } Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-24 00:03:46 UTC (rev 6919) @@ -21,6 +21,8 @@ lsp_test_speech.hh lsp_test_sonar.cc lsp_test_sonar.hh + lsp_test_fiducial.cc + lsp_test_fiducial.hh lsp_test_position2d.cc lsp_test_position2d.hh ) Added: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc 2008-07-24 00:03:46 UTC (rev 6919) @@ -0,0 +1,52 @@ +#include "lsp_test_fiducial.hh" + +using namespace lspTest; + +const int Fiducial::Samples = 361; + +void Fiducial::setUp() { + connect(); + fiducialProxy = playerc_fiducial_create( client, 0 ); + CPPUNIT_ASSERT( playerc_fiducial_subscribe( fiducialProxy, PLAYER_OPEN_MODE ) == 0 ); +} + + +void Fiducial::tearDown() { + CPPUNIT_ASSERT( playerc_fiducial_unsubscribe( fiducialProxy ) == 0 ); + playerc_fiducial_destroy( fiducialProxy ); + disconnect(); +} + +void Fiducial::testGeom() { + CPPUNIT_ASSERT( playerc_fiducial_get_geom( fiducialProxy ) == 0 ); + + // values from lsp_test.world + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -0.15, fiducialProxy->fiducial_geom.pose.px, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, fiducialProxy->fiducial_geom.pose.py, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, fiducialProxy->fiducial_geom.pose.pz, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, fiducialProxy->fiducial_geom.pose.ppitch, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, fiducialProxy->fiducial_geom.pose.proll, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (yaw)", 0, fiducialProxy->fiducial_geom.pose.pyaw, Delta ); +} + +void Fiducial::testData() { + playerc_client_read( client ); + + // verify that we're getting new data + fiducialProxy->info.fresh = 0; + playerc_client_read( client ); + CPPUNIT_ASSERT_MESSAGE( "fiducial updating", fiducialProxy->info.fresh == 1 ); + + CPPUNIT_ASSERT( fiducialProxy->info.datatime > 0 ); +// CPPUNIT_ASSERT_EQUAL_MESSAGE( "fiducials_count", 1, fiducialProxy->fiducials_count ); // lsp_test.world + + printf("\nfiducials_count: %d\n", fiducialProxy->fiducials_count ); + for ( int i = 0; i < fiducialProxy->fiducials_count; i++ ) { +// CPPUNIT_ASSERT( fiducialProxy->fiducials[i].id == 2 ); + printf( "fiducial return: %d @ [ %6.4f %6.4f %6.4f ]\n", + fiducialProxy->fiducials[i].id, + fiducialProxy->fiducials[i].pose.px, + fiducialProxy->fiducials[i].pose.py, + fiducialProxy->fiducials[i].pose.pyaw ); + } +} \ No newline at end of file Added: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh 2008-07-24 00:03:46 UTC (rev 6919) @@ -0,0 +1,33 @@ +#ifndef _LSP_FIDUCIAL_TEST_H_ +#define _LSP_FIDUCIAL_TEST_H_ + +#include <cppunit/extensions/HelperMacros.h> +#include <libplayerc/playerc.h> + +#include "lsp_test_proxy.hh" + +namespace lspTest { + class Fiducial : public Proxy + { + CPPUNIT_TEST_SUITE( Fiducial ); + CPPUNIT_TEST( testGeom ); + CPPUNIT_TEST( testData ); + CPPUNIT_TEST_SUITE_END(); + + protected: + playerc_fiducial_t* fiducialProxy; + + void testConfig(); + void testGeom(); + void testData(); + + static const int Samples; + public: + void setUp(); + void tearDown(); + }; +}; + +CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Fiducial ); + +#endif \ No newline at end of file Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-24 00:03:46 UTC (rev 6919) @@ -42,8 +42,8 @@ void Laser::testGeom() { CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 ); - // values from lsp_test.cfg/world and sick.inc - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, laserProxy->pose[0], Delta ); + // values from lsp_test.world and sick.inc + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0.03, laserProxy->pose[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, laserProxy->pose[2], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156, laserProxy->size[0], Delta ); Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-24 00:03:46 UTC (rev 6919) @@ -22,7 +22,7 @@ void Position2D::testGeom() { CPPUNIT_ASSERT( playerc_position2d_get_geom( posProxy ) == 0 ); - // values from lsp_test.cfg/world (pioneer2dx) + // values from pioneer.inc CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (x)", -0.04, posProxy->pose[0], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (y)", 0, posProxy->pose[1], Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (angle)", 0, posProxy->pose[2], Delta ); Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-24 00:03:46 UTC (rev 6919) @@ -12,7 +12,7 @@ CPPUNIT_TEST_SUB_SUITE( Position2D, Proxy ); CPPUNIT_TEST( testGeom ); CPPUNIT_TEST( testData ); - CPPUNIT_TEST( testMove ); +// CPPUNIT_TEST( testMove ); CPPUNIT_TEST_SUITE_END(); protected: Modified: code/stage/trunk/worlds/lsp_test.cfg =================================================================== --- code/stage/trunk/worlds/lsp_test.cfg 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-24 00:03:46 UTC (rev 6919) @@ -16,7 +16,7 @@ driver ( name "stage" - provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" ] + provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" "fiducial:0" ] model "r0" ) Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-24 00:00:30 UTC (rev 6918) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-24 00:03:46 UTC (rev 6919) @@ -26,13 +26,43 @@ bitmap "bitmaps/cave.png" ) -pioneer2dx +# extend the pioneer2dx definition from pioneer.inc +# +define trickedoutpioneer pioneer2dx +( + ranger() + + sicklaser( pose [ 0.03 0 0 0 ] ) + + fiducial + ( + pose [ -0.15 0 0 0 ] + range_max 8 + range_max_id 5 + ) + + blobfinder + ( + channel_count 6 + channels [ "red" "blue" "green" "cyan" "yellow" "magenta" ] + ) + + localization "odom" + localization_origin [ 0 0 0 ] +) + +trickedoutpioneer ( - # can refer to the robot by this name name "r0" - localization "odom" + fiducial_return 1 pose [ -7.490 -7.490 0 45.000 ] - sicklaser() ) +trickedoutpioneer +( + name "r1" + + fiducial_return 2 + pose [ -5.085 -7.193 0 144.304 ] +) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-24 11:45:08
|
Revision: 6922 http://playerstage.svn.sourceforge.net/playerstage/?rev=6922&view=rev Author: jeremy_asher Date: 2008-07-24 18:45:15 +0000 (Thu, 24 Jul 2008) Log Message: ----------- libstageplugin: finalized fiducial test Modified Paths: -------------- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh code/stage/trunk/worlds/lsp_test.world Modified: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc 2008-07-24 16:22:28 UTC (rev 6921) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc 2008-07-24 18:45:15 UTC (rev 6922) @@ -2,8 +2,6 @@ using namespace lspTest; -const int Fiducial::Samples = 361; - void Fiducial::setUp() { connect(); fiducialProxy = playerc_fiducial_create( client, 0 ); @@ -38,15 +36,19 @@ CPPUNIT_ASSERT_MESSAGE( "fiducial updating", fiducialProxy->info.fresh == 1 ); CPPUNIT_ASSERT( fiducialProxy->info.datatime > 0 ); -// CPPUNIT_ASSERT_EQUAL_MESSAGE( "fiducials_count", 1, fiducialProxy->fiducials_count ); // lsp_test.world + + // Make sure we see exactly 1 robot with ID 2 + CPPUNIT_ASSERT_EQUAL_MESSAGE( "fiducials_count", 1, fiducialProxy->fiducials_count ); // lsp_test.world + CPPUNIT_ASSERT( fiducialProxy->fiducials[0].id == 2 ); - printf("\nfiducials_count: %d\n", fiducialProxy->fiducials_count ); - for ( int i = 0; i < fiducialProxy->fiducials_count; i++ ) { -// CPPUNIT_ASSERT( fiducialProxy->fiducials[i].id == 2 ); - printf( "fiducial return: %d @ [ %6.4f %6.4f %6.4f ]\n", - fiducialProxy->fiducials[i].id, - fiducialProxy->fiducials[i].pose.px, - fiducialProxy->fiducials[i].pose.py, - fiducialProxy->fiducials[i].pose.pyaw ); - } + + +// printf("\nfiducials_count: %d\n", fiducialProxy->fiducials_count ); +// for ( int i = 0; i < fiducialProxy->fiducials_count; i++ ) { +// printf( "fiducial return: %d @ [ %6.4f %6.4f %6.4f ]\n", +// fiducialProxy->fiducials[i].id, +// fiducialProxy->fiducials[i].pose.px, +// fiducialProxy->fiducials[i].pose.py, +// fiducialProxy->fiducials[i].pose.pyaw ); +// } } \ No newline at end of file Modified: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh 2008-07-24 16:22:28 UTC (rev 6921) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh 2008-07-24 18:45:15 UTC (rev 6922) @@ -21,7 +21,6 @@ void testGeom(); void testData(); - static const int Samples; public: void setUp(); void tearDown(); Modified: code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-24 16:22:28 UTC (rev 6921) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-24 18:45:15 UTC (rev 6922) @@ -2,8 +2,6 @@ using namespace lspTest; -const int Sonar::Samples = 361; - void Sonar::setUp() { connect(); sonarProxy = playerc_sonar_create( client, 0 ); Modified: code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-24 16:22:28 UTC (rev 6921) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-24 18:45:15 UTC (rev 6922) @@ -21,7 +21,6 @@ void testGeom(); void testData(); - static const int Samples; public: void setUp(); void tearDown(); Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-24 16:22:28 UTC (rev 6921) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-24 18:45:15 UTC (rev 6922) @@ -65,4 +65,4 @@ fiducial_return 2 pose [ -5.085 -7.193 0 144.304 ] -) +) \ No newline at end of file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-24 16:28:50
|
Revision: 6925 http://playerstage.svn.sourceforge.net/playerstage/?rev=6925&view=rev Author: jeremy_asher Date: 2008-07-24 23:28:58 +0000 (Thu, 24 Jul 2008) Log Message: ----------- libstageplugin: progress on p_simulation and test, 2D and 3D get/set implemented, properties need work Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_simulation.cc code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/worlds/lsp_test.world Added Paths: ----------- code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-07-24 19:34:14 UTC (rev 6924) +++ code/stage/trunk/libstage/model.cc 2008-07-24 23:28:58 UTC (rev 6925) @@ -225,6 +225,7 @@ this->gui_mask = this->parent ? 0 : DEFAULT_MASK; this->callbacks = g_hash_table_new( g_int_hash, g_int_equal ); + g_datalist_init( &this->props ); this->flag_list = NULL; this->blinkenlights = g_ptr_array_new(); @@ -260,6 +261,8 @@ world->children = g_list_remove( world->children, this ); if( callbacks ) g_hash_table_destroy( callbacks ); + + g_datalist_clear( &props ); g_hash_table_remove( StgModel::modelsbyid, (void*)id ); Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2008-07-24 19:34:14 UTC (rev 6924) +++ code/stage/trunk/libstageplugin/p_driver.cc 2008-07-24 23:28:58 UTC (rev 6925) @@ -311,39 +311,31 @@ ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section ); break; -// case PLAYER_CAMERA_CODE: -// ifsrc = new InterfaceCamera( player_addr, this, cf, section ); -// break; + case PLAYER_FIDUCIAL_CODE: + ifsrc = new InterfaceFiducial( player_addr, this, cf, section ); + break; + case PLAYER_LASER_CODE: + ifsrc = new InterfaceLaser( player_addr, this, cf, section ); + break; + + case PLAYER_POSITION2D_CODE: + ifsrc = new InterfacePosition( player_addr, this, cf, section ); + break; case PLAYER_SIMULATION_CODE: ifsrc = new InterfaceSimulation( player_addr, this, cf, section ); - break; + break; - case PLAYER_LASER_CODE: - ifsrc = new InterfaceLaser( player_addr, this, cf, section ); - break; - - case PLAYER_POSITION2D_CODE: - ifsrc = new InterfacePosition( player_addr, this, cf, section ); - break; - case PLAYER_SONAR_CODE: ifsrc = new InterfaceSonar( player_addr, this, cf, section ); break; - -// case PLAYER_POWER_CODE: -// ifsrc = new InterfacePower( player_addr, this, cf, section ); -// break; + + case PLAYER_SPEECH_CODE: + ifsrc = new InterfaceSpeech( player_addr, this, cf, section ); + break; + -// case PLAYER_PTZ_CODE: -// ifsrc = new InterfacePtz( player_addr, this, cf, section ); -// break; - - case PLAYER_FIDUCIAL_CODE: - ifsrc = new InterfaceFiducial( player_addr, this, cf, section ); - break; - // case PLAYER_LOCALIZE_CODE: // ifsrc = new InterfaceLocalize( player_addr, this, cf, section ); // break; @@ -366,16 +358,25 @@ // case PLAYER_WIFI_CODE: // ifsrc = new InterfaceWifi( player_addr, this, cf, section ); -// break; +// break; + +// case PLAYER_CAMERA_CODE: +// ifsrc = new InterfaceCamera( player_addr, this, cf, section ); +// break; - case PLAYER_SPEECH_CODE: - ifsrc = new InterfaceSpeech( player_addr, this, cf, section ); - break; +// case PLAYER_POWER_CODE: +// ifsrc = new InterfacePower( player_addr, this, cf, section ); +// break; +// case PLAYER_PTZ_CODE: +// ifsrc = new InterfacePtz( player_addr, this, cf, section ); +// break; + // case PLAYER_BUMPER_CODE: // ifsrc = new InterfaceBumper( player_addr, this, cf, section ); // break; + default: PRINT_ERR1( "error: stage driver doesn't support interface type %d\n", player_addr.interf ); Modified: code/stage/trunk/libstageplugin/p_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/p_simulation.cc 2008-07-24 19:34:14 UTC (rev 6924) +++ code/stage/trunk/libstageplugin/p_simulation.cc 2008-07-24 23:28:58 UTC (rev 6925) @@ -46,7 +46,7 @@ // CODE ------------------------------------------------------------ -#define DEBUG +//#define DEBUG #include <libgen.h> // for dirname(3) #include <libplayercore/globals.h> // for player_argc & player_argv @@ -147,123 +147,226 @@ player_msghdr_t* hdr, void* data) { - // Is it a request to set a model's pose? - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_POSE2D, - this->addr)) - { - player_simulation_pose2d_req_t* req = - (player_simulation_pose2d_req_t*)data; + // Is it a request to get a model's pose in 2D? + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_GET_POSE2D, + this->addr)) + { + player_simulation_pose2d_req_t* req = + (player_simulation_pose2d_req_t*)data; + + PRINT_DEBUG1( "Stage: received request for the 2D position of object \"%s\"\n", req->name ); + + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); + + if( mod ) + { + stg_pose_t pose = mod->GetPose(); + + PRINT_DEBUG3( "Stage: returning location [ %.2f, %.2f, %.2f ]\n", + pose.x, pose.y, pose.a ); + + player_simulation_pose2d_req_t reply; + memcpy( &reply, req, sizeof(reply)); + reply.pose.px = pose.x; + reply.pose.py = pose.y; + reply.pose.pa = pose.a; + + this->driver->Publish( this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_GET_POSE2D, + (void*)&reply, sizeof(reply), NULL ); + return(0); + } + else + { + PRINT_WARN1( "Stage: GET_POSE2D request: simulation model \"%s\" not found", req->name ); + return(-1); + } + } + + // Is it a request to set a model's pose in 2D? + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_POSE2D, + this->addr)) + { + player_simulation_pose2d_req_t* req = + (player_simulation_pose2d_req_t*)data; - stg_pose_t pose; - pose.x = req->pose.px; - pose.y = req->pose.py; - pose.a = req->pose.pa; + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); - // look up the named model + if( mod ) + { + PRINT_DEBUG4( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f ]\n", + req->name, req->pose.px, req->pose.py, req->pose.pa ); + + stg_pose_t pose = mod->GetPose(); + pose.x = req->pose.px; + pose.y = req->pose.py; + pose.a = req->pose.pa; + + mod->SetPose( pose ); - StgModel* mod = StgDriver::world->GetModel( req->name ); - - if( mod ) - { - printf( "Stage: moving \"%s\" to (%.2f,%.2f,%.2f)\n", - req->name, pose.x, pose.y, pose.a ); + this->driver->Publish(this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_SET_POSE2D); + return(0); + } + else + { + PRINT_WARN1( "SETPOSE2D request: simulation model \"%s\" not found", req->name ); + return(-1); + } + } - mod->SetPose( pose ); - - this->driver->Publish(this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_SIMULATION_REQ_SET_POSE2D); - return(0); - } - else - { - PRINT_WARN1( "SETPOSE2D request: simulation model \"%s\" not found", req->name ); - return(-1); - } - } - // Is it a request to set a model's pose? - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_SET_PROPERTY, - this->addr)) + // Is it a request to get a model's pose in 3D? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_GET_POSE3D, + this->addr)) { - - player_simulation_property_req_t* req = - (player_simulation_property_req_t*)data; - - // look up the named model - StgModel* mod = - //stg_world_model_name_lookup( StgDriver::world, req->name ); - StgDriver::world->GetModel( req->name ); - - if( mod ) - { - int ack = - mod->SetProperty( req->prop, - (void*)req->value ); - - this->driver->Publish(this->addr, resp_queue, - ack==0 ? PLAYER_MSGTYPE_RESP_ACK : PLAYER_MSGTYPE_RESP_NACK, - PLAYER_SIMULATION_REQ_SET_PROPERTY); - return(0); + player_simulation_pose3d_req_t* req = + (player_simulation_pose3d_req_t*)data; + + PRINT_DEBUG1( "Stage: received request for the 3D position of object \"%s\"\n", req->name ); + + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); + + if( mod ) + { + stg_pose_t pose = mod->GetPose(); + + PRINT_DEBUG4( "Stage: returning location [ %.2f, %.2f, %.2f, %.2f ]\n", + pose.x, pose.y, pose.z, pose.a ); + + player_simulation_pose3d_req_t reply; + memcpy( &reply, req, sizeof(reply)); + reply.pose.px = pose.x; + reply.pose.py = pose.y; + reply.pose.pz = pose.z; + reply.pose.proll = 0; // currently unused + reply.pose.ppitch = 0; // currently unused + reply.pose.pyaw = pose.a; + reply.simtime = mod->GetWorld()->SimTimeNow(); // time in microseconds + + this->driver->Publish( this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_GET_POSE3D, + (void*)&reply, sizeof(reply), NULL ); + return(0); + } + else + { + PRINT_WARN1( "Stage: GET_POSE3D request: simulation model \"%s\" not found", req->name ); + return(-1); + } } - else + + // Is it a request to set a model's pose in 3D? + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_POSE3D, + this->addr)) { - PRINT_WARN1( "SET_PROPERTY request: simulation model \"%s\" not found", req->name ); - return(-1); + player_simulation_pose3d_req_t* req = + (player_simulation_pose3d_req_t*)data; + + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); + + if( mod ) + { + PRINT_DEBUG5( "Stage: moving \"%s\" to [ %.2f, %.2f, %.2f %.2f ]\n", + req->name, req->pose.px, req->pose.py, req->pose.pz, req->pose.pyaw ); + + stg_pose_t pose = mod->GetPose(); + pose.x = req->pose.px; + pose.y = req->pose.py; + pose.z = req->pose.pz; + pose.a = req->pose.pyaw; + // roll and pitch are unused + + mod->SetPose( pose ); + + this->driver->Publish(this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_SET_POSE3D); + return(0); + } + else + { + PRINT_WARN1( "SETPOSE2D request: simulation model \"%s\" not found", req->name ); + return(-1); + } } - } - // Is it a request to get a model's pose? - else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_SIMULATION_REQ_GET_POSE2D, - this->addr)) + + + // Is it a request to set a model's property? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_SET_PROPERTY, + this->addr)) { - player_simulation_pose2d_req_t* req = - (player_simulation_pose2d_req_t*)data; + player_simulation_property_req_t* req = + (player_simulation_property_req_t*)data; - printf( "Stage: received request for position of object \"%s\"\n", req->name ); - - // look up the named model - StgModel* mod = - //stg_world_model_name_lookup( StgDriver::world, req->name ); - StgDriver::world->GetModel( req->name ); - - if( mod ) - { - stg_pose_t pose = mod->GetPose(); + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); + + if( mod ) + { + int ack = + mod->SetProperty( req->prop, + (void*)req->value ); - printf( "Stage: returning location (%.2f,%.2f,%.2f)\n", - pose.x, pose.y, pose.a ); + this->driver->Publish(this->addr, resp_queue, + ack==0 ? PLAYER_MSGTYPE_RESP_ACK : PLAYER_MSGTYPE_RESP_NACK, + PLAYER_SIMULATION_REQ_SET_PROPERTY); + return(0); + } + else + { + PRINT_WARN1( "SET_PROPERTY request: simulation model \"%s\" not found", req->name ); + return(-1); + } + } - - player_simulation_pose2d_req_t reply; - memcpy( &reply, req, sizeof(reply)); - reply.pose.px = pose.x; - reply.pose.py = pose.y; - reply.pose.pa = pose.a; - - /* - printf( "Stage: returning location (%d %d %d)\n", - reply.x, reply.y, reply.a ); - */ - - this->driver->Publish( this->addr, resp_queue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_SIMULATION_REQ_GET_POSE2D, - (void*)&reply, sizeof(reply), NULL ); - return(0); - } - else + // Is it a request to get a model's property? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_SIMULATION_REQ_GET_PROPERTY, + this->addr)) { - PRINT_WARN1( "Stage: GET_POSE2D request: simulation model \"%s\" not found", req->name ); - return(-1); - } - } - else - { - // Don't know how to handle this message. - PRINT_WARN2( "stg_simulation doesn't support msg with type/subtype %d/%d", - hdr->type, hdr->subtype); - return(-1); - } + player_simulation_property_req_t* req = + (player_simulation_property_req_t*)data; + + // look up the named model + StgModel* mod = StgDriver::world->GetModel( req->name ); + + if( mod ) + { + + req->value = (char*)malloc( 2 ); + req->value[0] = NULL; + //(char*)mod->GetProperty( req->prop ); + + this->driver->Publish(this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_SIMULATION_REQ_GET_PROPERTY); + return(0); + } + else + { + PRINT_WARN1( "GET_PROPERTY request: simulation model \"%s\" not found", req->name ); + return(-1); + } + } + + + else + { + // Don't know how to handle this message. + PRINT_WARN2( "stg_simulation doesn't support msg with type/subtype %d/%d", + hdr->type, hdr->subtype); + return(-1); + } } Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-24 19:34:14 UTC (rev 6924) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-24 23:28:58 UTC (rev 6925) @@ -15,10 +15,12 @@ lsp_test.cc lsp_test_proxy.cc lsp_test_proxy.hh + lsp_test_simulation.cc + lsp_test_simulation.hh + lsp_test_speech.cc + lsp_test_speech.hh lsp_test_laser.cc lsp_test_laser.hh - lsp_test_speech.cc - lsp_test_speech.hh lsp_test_sonar.cc lsp_test_sonar.hh lsp_test_fiducial.cc Added: code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc 2008-07-24 23:28:58 UTC (rev 6925) @@ -0,0 +1,88 @@ +#include "lsp_test_simulation.hh" + +using namespace lspTest; + +void Simulation::setUp() { + connect(); + simProxy = playerc_simulation_create( client, 0 ); + CPPUNIT_ASSERT( playerc_simulation_subscribe( simProxy, PLAYER_OPEN_MODE ) == 0 ); +} + +void Simulation::tearDown() { + CPPUNIT_ASSERT( playerc_simulation_unsubscribe( simProxy ) == 0 ); + playerc_simulation_destroy( simProxy ); + disconnect(); +} + +void Simulation::testPose2D() { + double x, y, a; + double x2, y2, a2; + + // See if the robot "r1" is where it should be according to lsp_test.world + CPPUNIT_ASSERT( playerc_simulation_get_pose2d( simProxy, "r1", &x, &y, &a ) == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -5.085, x, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (a)", 3*M_PI/4, a, Delta ); + + // Set pose to [ 0, 0, 0 ] and verify + CPPUNIT_ASSERT( playerc_simulation_set_pose2d( simProxy, "r1", 0, 0, 0 ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_get_pose2d( simProxy, "r1", &x2, &y2, &a2 ) == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, x2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, y2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (a)", 0, a2, Delta ); + + // Return the robot to its starting point + CPPUNIT_ASSERT( playerc_simulation_set_pose2d( simProxy, "r1", x, y, a ) == 0 ); +} + +void Simulation::testPose3D() { + double x, y, z, roll, pitch, yaw, time; + double x2, y2, z2, roll2, pitch2, yaw2, time2; + + // See if the robot "r1" is where it should be according to lsp_test.world + CPPUNIT_ASSERT( playerc_simulation_get_pose3d( simProxy, "r1", &x, &y, &z, &roll, &pitch, &yaw, &time ) == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -5.085, x, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, z, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, roll, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, pitch, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (yaw)", 3*M_PI/4, yaw, Delta ); + CPPUNIT_ASSERT( time > 0 ); + + // Set pose to [ 0, 0, 0.5, M_PI/4, M_PI/4, M_PI/4 ] and verify + CPPUNIT_ASSERT( playerc_simulation_set_pose3d( simProxy, "r1", 0, 0, 0.5, M_PI/4, M_PI/4, M_PI/4 ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_get_pose3d( simProxy, "r1", &x2, &y2, &z2, &roll2, &pitch2, &yaw2, &time2 ) == 0 ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, x2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, y2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0.5, z2, Delta ); + // roll and pitch are currently unused in stage ( returns set to 0 ) + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, roll2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, pitch2, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (yaw)", M_PI/4, yaw2, Delta ); + CPPUNIT_ASSERT( time2 > time ); + + // Return the robot to its starting point + CPPUNIT_ASSERT( playerc_simulation_set_pose3d( simProxy, "r1", x, y, z, roll, pitch, yaw ) == 0 ); +} + +void Simulation::testProperties() { + int r0Agg = 5; + int r1Agg = 1; + int r1Pow = 125; + + // Set some properties + CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r0", "aggression", &r0Agg, sizeof(r0Agg) ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r1", "aggression", &r1Agg, sizeof(r0Agg) ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r1", "power", &r1Pow, sizeof(r1Pow) ) == 0 ); + + // Get the properties back + int r0Agg2, r1Agg2, r1Pow2; + CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r0", "aggression", &r0Agg2, sizeof(r0Agg2) ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r1", "aggression", &r1Agg2, sizeof(r0Agg2) ) == 0 ); + CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r1", "power", &r1Pow2, sizeof(r1Pow2) ) == 0 ); + + // Make sure they're the same + CPPUNIT_ASSERT( r0Agg == r0Agg2 ); + CPPUNIT_ASSERT( r1Agg == r1Agg2 ); + CPPUNIT_ASSERT( r1Pow == r1Pow2 ); +} \ No newline at end of file Added: code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh 2008-07-24 23:28:58 UTC (rev 6925) @@ -0,0 +1,33 @@ +#ifndef _LSP_SIMULATION_TEST_H_ +#define _LSP_SIMULATION_TEST_H_ + +#include <cppunit/extensions/HelperMacros.h> +#include <libplayerc/playerc.h> + +#include "lsp_test_proxy.hh" + +namespace lspTest { + class Simulation : public Proxy + { + CPPUNIT_TEST_SUB_SUITE( Simulation, Proxy ); + CPPUNIT_TEST( testPose2D ); + CPPUNIT_TEST( testPose3D ); + CPPUNIT_TEST( testProperties ); + CPPUNIT_TEST_SUITE_END(); + + protected: + playerc_simulation_t* simProxy; + + void testPose2D(); + void testPose3D(); + void testProperties(); + + public: + void setUp(); + void tearDown(); + }; +}; + +CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Simulation ); + +#endif \ No newline at end of file Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-24 19:34:14 UTC (rev 6924) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-24 23:28:58 UTC (rev 6925) @@ -64,5 +64,5 @@ name "r1" fiducial_return 2 - pose [ -5.085 -7.193 0 144.304 ] + pose [ -5.085 -7.193 0 135 ] ) \ No newline at end of file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-25 14:27:51
|
Revision: 6927 http://playerstage.svn.sourceforge.net/playerstage/?rev=6927&view=rev Author: jeremy_asher Date: 2008-07-25 21:27:59 +0000 (Fri, 25 Jul 2008) Log Message: ----------- libstageplugin: fixed blobfinder returning pixels in reverse x order, implemented blobfinder test, added readme for tests Modified Paths: -------------- code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstageplugin/p_blobfinder.cc code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc code/stage/trunk/worlds/lsp_test.cfg code/stage/trunk/worlds/lsp_test.world Added Paths: ----------- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh code/stage/trunk/libstageplugin/test/readme.txt Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 21:27:59 UTC (rev 6927) @@ -197,7 +197,7 @@ //if( samples[s].block->Color() != 0xFFFF0000 ) //continue; // we saw nothing - unsigned int blobleft = s; + unsigned int right = s; stg_color_t blobcol = samples[s].block->Color(); //printf( "blob start %d color %X\n", blobleft, blobcol ); @@ -212,7 +212,7 @@ s++; } - unsigned int blobright = s-1; + unsigned int left = s - 1; //if we have color filters in place, check to see if we're looking for this color if( colors->len ) @@ -235,25 +235,25 @@ // find the average range to the blob; stg_meters_t range = 0; - for( unsigned int t=blobleft; t<=blobright; t++ ) + for( unsigned int t=right; t<=left; t++ ) range += samples[t].range; - range /= blobright-blobleft + 1; + range /= left-right + 1; double startyangle = atan2( robotHeight/2.0, range ); double endyangle = -startyangle; int blobtop = scan_height/2 - (int)(startyangle/yRadsPerPixel); int blobbottom = scan_height/2 -(int)(endyangle/yRadsPerPixel); - blobtop = MIN( blobtop, (int)scan_height ); + blobtop = MAX( blobtop, 0 ); blobbottom = MIN( blobbottom, (int)scan_height ); // fill in an array entry for this blob stg_blobfinder_blob_t blob; memset( &blob, 0, sizeof(blob) ); blob.color = blobcol; - blob.left = blobleft; + blob.left = scan_width - left - 1; blob.top = blobtop; - blob.right = blobright; + blob.right = scan_width - right - 1;; blob.bottom = blobbottom; blob.range = range; @@ -346,6 +346,7 @@ PushColor( 0xFF000000 ); glRectf( 0,0, scan_width, scan_height ); PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); // draw the blobs on the screen for( unsigned int s=0; s<blobs->len; s++ ) @@ -360,7 +361,6 @@ PopColor(); } - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glPopMatrix(); } Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 21:27:59 UTC (rev 6927) @@ -73,20 +73,19 @@ for( b=0; b<bcount; b++ ) { // useful debug - leave in - /* + cout << "blob " - << " channel: " << (int)blobs[b].channel - << " area: " << blobs[b].area << " left: " << blobs[b].left << " right: " << blobs[b].right << " top: " << blobs[b].top << " bottom: " << blobs[b].bottom + << " color: " << hex << blobs[b].color << dec << endl; - */ + int dx = blobs[b].right - blobs[b].left; int dy = blobs[b].top - blobs[b].bottom; - + bfd.blobs[b].x = blobs[b].left + dx/2; bfd.blobs[b].y = blobs[b].bottom + dy/2; Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-25 21:27:59 UTC (rev 6927) @@ -15,6 +15,8 @@ lsp_test.cc lsp_test_proxy.cc lsp_test_proxy.hh + lsp_test_blobfinder.cc + lsp_test_blobfinder.hh lsp_test_simulation.cc lsp_test_simulation.hh lsp_test_speech.cc Added: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc 2008-07-25 21:27:59 UTC (rev 6927) @@ -0,0 +1,59 @@ +#include "lsp_test_blobfinder.hh" + +using namespace lspTest; + +void Blobfinder::setUp() { + connect(); + blobProxy = playerc_blobfinder_create( client, 0 ); + CPPUNIT_ASSERT( playerc_blobfinder_subscribe( blobProxy, PLAYER_OPEN_MODE ) == 0 ); +} + + +void Blobfinder::tearDown() { + CPPUNIT_ASSERT( playerc_blobfinder_unsubscribe( blobProxy ) == 0 ); + playerc_blobfinder_destroy( blobProxy ); + disconnect(); +} + +void Blobfinder::testData() { + playerc_client_read( client ); + + // verify that we're getting new data + blobProxy->info.fresh = 0; + playerc_client_read( client ); + CPPUNIT_ASSERT_MESSAGE( "blobfinder updating", blobProxy->info.fresh == 1 ); + + CPPUNIT_ASSERT( blobProxy->info.datatime > 0 ); + + //printf("\ncolor0: 0x%X, color1: 0x%X\n", blobProxy->blobs[0].color, blobProxy->blobs[1].color); + + // check stage defaults + CPPUNIT_ASSERT( blobProxy->width == 80 ); + CPPUNIT_ASSERT( blobProxy->height == 60 ); + + // Make sure we see three blobs: wall, robot, wall + CPPUNIT_ASSERT_EQUAL_MESSAGE( "blobs_count", (unsigned int)3, blobProxy->blobs_count ); // lsp_test.world + // sanity checks + CPPUNIT_ASSERT( blobProxy->blobs[1].range > 0 ); + CPPUNIT_ASSERT( blobProxy->blobs[1].area > 0 ); + CPPUNIT_ASSERT( blobProxy->blobs[1].y > 0 ); + CPPUNIT_ASSERT( blobProxy->blobs[1].left < blobProxy->blobs[1].right ); + CPPUNIT_ASSERT( blobProxy->blobs[1].top < blobProxy->blobs[1].bottom ); + + // robot should be red + CPPUNIT_ASSERT( blobProxy->blobs[1].color == 0xFFFF0000 ); + + // robot should be closer + CPPUNIT_ASSERT( blobProxy->blobs[0].range > blobProxy->blobs[1].range ); + + // robot should be towards the right + uint32_t robotCenter = blobProxy->blobs[1].x; + uint32_t imageCenter = blobProxy->width / 2; + //printf(" robotCenter: %d, imageCenter: %d\n", robotCenter, imageCenter); + CPPUNIT_ASSERT( robotCenter > imageCenter ); + + + +// CPPUNIT_ASSERT_EQUAL_MESSAGE( "id", (unsigned int)2, blobProxy->blobs[0].id ); + +} \ No newline at end of file Added: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh (rev 0) +++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh 2008-07-25 21:27:59 UTC (rev 6927) @@ -0,0 +1,31 @@ +#ifndef _LSP_BLOBFINDER_TEST_H_ +#define _LSP_BLOBFINDER_TEST_H_ + +#include <cppunit/extensions/HelperMacros.h> +#include <libplayerc/playerc.h> + +#include "lsp_test_proxy.hh" + +namespace lspTest { + class Blobfinder : public Proxy + { + CPPUNIT_TEST_SUITE( Blobfinder ); + CPPUNIT_TEST( testData ); + CPPUNIT_TEST_SUITE_END(); + + protected: + playerc_blobfinder_t* blobProxy; + + void testConfig(); + void testGeom(); + void testData(); + + public: + void setUp(); + void tearDown(); + }; +}; + +CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Blobfinder ); + +#endif \ No newline at end of file Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-25 21:27:59 UTC (rev 6927) @@ -12,7 +12,7 @@ CPPUNIT_TEST_SUB_SUITE( Position2D, Proxy ); CPPUNIT_TEST( testGeom ); CPPUNIT_TEST( testData ); -// CPPUNIT_TEST( testMove ); + CPPUNIT_TEST( testMove ); CPPUNIT_TEST_SUITE_END(); protected: Modified: code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc 2008-07-25 21:27:59 UTC (rev 6927) @@ -20,8 +20,8 @@ // See if the robot "r1" is where it should be according to lsp_test.world CPPUNIT_ASSERT( playerc_simulation_get_pose2d( simProxy, "r1", &x, &y, &a ) == 0 ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -5.085, x, Delta ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -4.19, x, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -5.71, y, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (a)", 3*M_PI/4, a, Delta ); // Set pose to [ 0, 0, 0 ] and verify @@ -41,8 +41,8 @@ // See if the robot "r1" is where it should be according to lsp_test.world CPPUNIT_ASSERT( playerc_simulation_get_pose3d( simProxy, "r1", &x, &y, &z, &roll, &pitch, &yaw, &time ) == 0 ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -5.085, x, Delta ); - CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -4.19, x, Delta ); + CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -5.71, y, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, z, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, roll, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, pitch, Delta ); Added: code/stage/trunk/libstageplugin/test/readme.txt =================================================================== --- code/stage/trunk/libstageplugin/test/readme.txt (rev 0) +++ code/stage/trunk/libstageplugin/test/readme.txt 2008-07-25 21:27:59 UTC (rev 6927) @@ -0,0 +1,9 @@ +Building +-------- +CPPUNIT is required. Build by enabling the BUILD_LSPTEST option either with a cmake command line arg or through ccmake on the root of the stage source. + +Execution +--------- +Run player on lsp_test.cfg, included in the worlds section of the stage source. + +Execute lsp_test on the same machine and the test suites will connect to localhost:6665 to perform testing. \ No newline at end of file Modified: code/stage/trunk/worlds/lsp_test.cfg =================================================================== --- code/stage/trunk/worlds/lsp_test.cfg 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-25 21:27:59 UTC (rev 6927) @@ -16,7 +16,7 @@ driver ( name "stage" - provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" "fiducial:0" ] + provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" "fiducial:0" "blobfinder:0" ] model "r0" ) Modified: code/stage/trunk/worlds/lsp_test.world =================================================================== --- code/stage/trunk/worlds/lsp_test.world 2008-07-25 17:35:38 UTC (rev 6926) +++ code/stage/trunk/worlds/lsp_test.world 2008-07-25 21:27:59 UTC (rev 6927) @@ -32,11 +32,11 @@ ( ranger() - sicklaser( pose [ 0.03 0 0 0 ] ) + sicklaser( pose [ 0.030 0 0 0 ] ) fiducial ( - pose [ -0.15 0 0 0 ] + pose [ -0.150 0 0 0 ] range_max 8 range_max_id 5 ) @@ -56,6 +56,7 @@ name "r0" fiducial_return 1 + blobfinder_return 1 pose [ -7.490 -7.490 0 45.000 ] ) @@ -64,5 +65,6 @@ name "r1" fiducial_return 2 - pose [ -5.085 -7.193 0 135 ] + blobfinder_return 2 + pose [ -4.19 -5.71 0 135.000 ] ) \ No newline at end of file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-28 16:29:17
|
Revision: 6931 http://playerstage.svn.sourceforge.net/playerstage/?rev=6931&view=rev Author: jeremy_asher Date: 2008-07-28 23:29:25 +0000 (Mon, 28 Jul 2008) Log Message: ----------- stage: some CMake script fixes, OpenGL should be located correctly now Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstageplugin/test/CMakeLists.txt code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh code/stage/trunk/libstageplugin/test/lsp_test_laser.cc code/stage/trunk/libstageplugin/test/lsp_test_laser.hh code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh code/stage/trunk/libstageplugin/test/lsp_test_speech.hh Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) @@ -41,6 +41,10 @@ pkg_search_module( LIBPNG REQUIRED libpng ) find_package( OpenGL REQUIRED ) +IF (NOT OPENGL_GLU_FOUND) + message(FATAL_ERROR "GLU not found, aborting") +ENDIF (NOT OPENGL_GLU_FOUND) + # Look for player v${MIN_PLAYER} or higher and set flags # built-ins don't work properly, so use pkg-config directly FIND_PROGRAM (PKGCONFIG NAMES pkg-config) @@ -98,8 +102,8 @@ # all targets need these library directories link_directories(${GLIB_LIBRARY_DIRS} - ${LIBPNG_LIBRARY_DIRS} - ${OPENGL_LIBRARY_DIRS} + ${GLIB_LIBRARY_DIRS} + ${LIBPNG_LIBRARY_DIRS} ) # work through these subdirs Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) @@ -1,3 +1,15 @@ +find_path( LTDL_INCLUDE_DIR ltdl.h DOC "Libtool include dir" ) +find_library( LTDL_LIB ltdl DOC "Libtool lib" ) + +IF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") + message( FATAL_ERROR "Libtool library not found, aborting" ) +ENDIF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") + +include_directories( + ${OPENGL_INCLUDE_DIR} + ${LTDL_INCLUDE_DIR} +) + set( stageSrcs ancestor.cc block.cc camera.cc @@ -37,7 +49,7 @@ ${GLIB_LIBRARIES} ${OPENGL_LIBRARIES} ${APPLE_LIBRARIES} - ltdl + ${LTDL_LIB} ) # When linking stage, pass LDFLAGS from fltk-config Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) @@ -1,17 +1,25 @@ -MESSAGE( STATUS "Building Player plugin tests" ) +message( STATUS "Building Player plugin tests" ) -pkg_search_module( PLAYERC REQUIRED playerc ) pkg_search_module( CPPUNIT REQUIRED cppunit ) +find_path( PLAYERC_INCLUDE_DIR libplayerc/playerc.h DOC "libplayerc include dir" ) +find_library( PLAYERC_LIB playerc DOC "libplayerc lib" ) + +IF (${PLAYERC_LIB} STREQUAL "PLAYERC_LIB-NOTFOUND") + message( FATAL_ERROR "libplayerc library not found, aborting" ) +ENDIF (${PLAYERC_LIB} STREQUAL "PLAYERC_LIB-NOTFOUND") + include_directories( - ${CPPUNIT_INCLUDE_PATH} + ${CPPUNIT_INCLUDE_DIRS} + ${PLAYERC_INCLUDE_DIR} ) -link_directories( +link_directories( ${CPPUNIT_LIBRARY_DIRS} ) -SET( lspTestSrcs + +set( lspTestSrcs lsp_test.cc lsp_test_proxy.cc lsp_test_proxy.hh @@ -32,7 +40,9 @@ ) add_executable( lsp_test ${lspTestSrcs} ) + target_link_libraries( lsp_test - ${PLAYERC_LIBRARIES} + ${PLAYERC_LIB} ${CPPUNIT_LIBRARIES} + dl ) Modified: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -56,4 +56,4 @@ // CPPUNIT_ASSERT_EQUAL_MESSAGE( "id", (unsigned int)2, blobProxy->blobs[0].id ); -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -28,4 +28,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Blobfinder ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -51,4 +51,4 @@ // fiducialProxy->fiducials[i].pose.py, // fiducialProxy->fiducials[i].pose.pyaw ); // } -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_fiducial.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -29,4 +29,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Fiducial ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -72,4 +72,4 @@ // printf("[%6.3f, %6.3f ] \n", laserProxy->scan[i][0], laserProxy->scan[i][1]); } -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -31,4 +31,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Laser ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -114,4 +114,4 @@ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py, Delta ); CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa, Delta ); -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -30,4 +30,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Position2D ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -14,4 +14,4 @@ void Proxy::disconnect() { CPPUNIT_ASSERT( playerc_client_disconnect( client ) == 0 ); playerc_client_destroy( client ); -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -20,4 +20,4 @@ }; }; -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_simulation.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -30,4 +30,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Simulation ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-28 23:29:25 UTC (rev 6931) @@ -42,4 +42,4 @@ CPPUNIT_ASSERT( sonarProxy->scan[i] >= 0 ); // printf(" range[%d]: %6.3f ]\n", i, sonarProxy->scan[i]); } -} \ No newline at end of file +} Modified: code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -29,4 +29,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Sonar ); -#endif \ No newline at end of file +#endif Modified: code/stage/trunk/libstageplugin/test/lsp_test_speech.hh =================================================================== --- code/stage/trunk/libstageplugin/test/lsp_test_speech.hh 2008-07-27 02:59:19 UTC (rev 6930) +++ code/stage/trunk/libstageplugin/test/lsp_test_speech.hh 2008-07-28 23:29:25 UTC (rev 6931) @@ -26,4 +26,4 @@ CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Speech ); -#endif \ No newline at end of file +#endif This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-29 11:57:01
|
Revision: 6932 http://playerstage.svn.sourceforge.net/playerstage/?rev=6932&view=rev Author: jeremy_asher Date: 2008-07-29 18:57:10 +0000 (Tue, 29 Jul 2008) Log Message: ----------- stage: getting ready for 3.0.1 Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/docsrc/stage.dox code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/worldgui.cc code/stage/trunk/libstageplugin/test/CMakeLists.txt Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) +++ code/stage/trunk/CMakeLists.txt 2008-07-29 18:57:10 UTC (rev 6932) @@ -2,7 +2,7 @@ SET( V_MAJOR 3 ) SET( V_MINOR 0 ) -SET( V_BUGFIX 0 ) +SET( V_BUGFIX 1 ) SET( VERSION ${V_MAJOR}.${V_MINOR}.${V_BUGFIX} ) SET( APIVERSION ${V_MAJOR}.${V_MINOR} ) Modified: code/stage/trunk/docsrc/stage.dox =================================================================== --- code/stage/trunk/docsrc/stage.dox 2008-07-28 23:29:25 UTC (rev 6931) +++ code/stage/trunk/docsrc/stage.dox 2008-07-29 18:57:10 UTC (rev 6932) @@ -23,7 +23,7 @@ # This could be handy for archiving the generated documentation or # if some version control system is used. -PROJECT_NUMBER = 3.0.0 +PROJECT_NUMBER = 3.0.1 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) # base path where the generated documentation will be put. Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-07-29 18:57:10 UTC (rev 6932) @@ -1,3 +1,4 @@ +message( STATUS "Checking for libtool" ) find_path( LTDL_INCLUDE_DIR ltdl.h DOC "Libtool include dir" ) find_library( LTDL_LIB ltdl DOC "Libtool lib" ) @@ -2,3 +3,5 @@ IF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") - message( FATAL_ERROR "Libtool library not found, aborting" ) + message( FATAL_ERROR "libtool library not found, aborting" ) +ELSE (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") + message( STATUS " found" ) ENDIF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-07-28 23:29:25 UTC (rev 6931) +++ code/stage/trunk/libstage/worldgui.cc 2008-07-29 18:57:10 UTC (rev 6932) @@ -326,7 +326,7 @@ std::string StgWorldGui::ClockString() { - const uint32_t usec_per_hour = 3600000000; + const uint32_t usec_per_hour = 3600000000U; const uint32_t usec_per_minute = 60000000; const uint32_t usec_per_second = 1000000; const uint32_t usec_per_msec = 1000; Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-28 23:29:25 UTC (rev 6931) +++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-29 18:57:10 UTC (rev 6932) @@ -1,24 +1,18 @@ message( STATUS "Building Player plugin tests" ) pkg_search_module( CPPUNIT REQUIRED cppunit ) +pkg_search_module( PLAYERC REQUIRED playerc ) -find_path( PLAYERC_INCLUDE_DIR libplayerc/playerc.h DOC "libplayerc include dir" ) -find_library( PLAYERC_LIB playerc DOC "libplayerc lib" ) - -IF (${PLAYERC_LIB} STREQUAL "PLAYERC_LIB-NOTFOUND") - message( FATAL_ERROR "libplayerc library not found, aborting" ) -ENDIF (${PLAYERC_LIB} STREQUAL "PLAYERC_LIB-NOTFOUND") - include_directories( ${CPPUNIT_INCLUDE_DIRS} - ${PLAYERC_INCLUDE_DIR} + ${PLAYERC_INCLUDE_DIRS} ) link_directories( ${CPPUNIT_LIBRARY_DIRS} + ${PLAYERC_LIBRARY_DIRS} ) - set( lspTestSrcs lsp_test.cc lsp_test_proxy.cc @@ -42,7 +36,7 @@ add_executable( lsp_test ${lspTestSrcs} ) target_link_libraries( lsp_test - ${PLAYERC_LIB} + ${PLAYERC_LIBRARIES} ${CPPUNIT_LIBRARIES} dl ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-29 16:06:59
|
Revision: 6934 http://playerstage.svn.sourceforge.net/playerstage/?rev=6934&view=rev Author: jeremy_asher Date: 2008-07-29 23:07:08 +0000 (Tue, 29 Jul 2008) Log Message: ----------- stage: more build script tweaks to handle exotic fltk location issues Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/RELEASE.txt Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-29 20:26:46 UTC (rev 6933) +++ code/stage/trunk/CMakeLists.txt 2008-07-29 23:07:08 UTC (rev 6934) @@ -39,47 +39,59 @@ include(FindPkgConfig) pkg_search_module( GLIB REQUIRED glib-2.0 ) pkg_search_module( LIBPNG REQUIRED libpng ) +MESSAGE( STATUS "Checking for OpenGL" ) find_package( OpenGL REQUIRED ) +IF ( OPENGL_FOUND ) + IF ( NOT OPENGL_GLU_FOUND ) + message( FATAL_ERROR "GLU not found, aborting" ) + ELSE ( NOT OPENGL_GLU_FOUND ) + MESSAGE( STATUS "OpenGL found" ) + ENDIF ( NOT OPENGL_GLU_FOUND ) +ELSE ( OPENGL_FOUND ) + MESSAGE( FATAL_ERROR "OpenGL not found, aborting" ) +ENDIF ( OPENGL_FOUND ) -IF (NOT OPENGL_GLU_FOUND) - message(FATAL_ERROR "GLU not found, aborting") -ENDIF (NOT OPENGL_GLU_FOUND) - # Look for player v${MIN_PLAYER} or higher and set flags # built-ins don't work properly, so use pkg-config directly -FIND_PROGRAM (PKGCONFIG NAMES pkg-config) -IF (PKGCONFIG) +MESSAGE( STATUS "Checking for Player" ) +IF ( NOT PKG_CONFIG ) + FIND_PROGRAM ( PKG_CONFIG NAMES pkg-config ) +ENDIF ( NOT PKG_CONFIG ) +IF ( PKG_CONFIG ) EXECUTE_PROCESS (COMMAND pkg-config --atleast-version=${MIN_PLAYER} playercore --silence-errors RESULT_VARIABLE PLAYER_NOT_FOUND) IF (PLAYER_NOT_FOUND EQUAL 0) # found the correct version pkg_search_module( PLAYER playercore ) - MESSAGE(STATUS "Player headers found in ${PLAYER_INCLUDE_DIRS}") - MESSAGE(STATUS "Player libs found in ${PLAYER_LIBRARY_DIRS}") + MESSAGE(STATUS " Player headers found in ${PLAYER_INCLUDE_DIRS}") + MESSAGE(STATUS " Player libs found in ${PLAYER_LIBRARY_DIRS}") SET (PLAYER_FOUND TRUE) ELSE (PLAYER_NOT_FOUND EQUAL 0) - MESSAGE(STATUS "Player >=v${MIN_PLAYER} not found, skipping Player plugin") + MESSAGE(STATUS " Player >=v${MIN_PLAYER} not found, skipping Player plugin") SET (PLAYER_FOUND FALSE) ENDIF (PLAYER_NOT_FOUND EQUAL 0) -ELSE (PKGCONFIG) - MESSAGE (STATUS "pkg-config not found, skipping Player plugin") -ENDIF (PKGCONFIG) +ELSE ( PKG_CONFIG ) + MESSAGE (STATUS " pkg-config not found, skipping Player plugin. If pkg-config should be available, please specify its full path using the PKG_CONFIG variable.") +ENDIF ( PKG_CONFIG ) # find FLTK and set flags -FIND_PROGRAM (FLTKCONFIG NAMES fltk-config) -IF (FLTKCONFIG) - EXECUTE_PROCESS (COMMAND fltk-config --cxxflags --use-gl --use-images +MESSAGE( STATUS "Looking for FLTK" ) +IF ( NOT FLTK_CONFIG ) + FIND_PROGRAM ( FLTK_CONFIG NAMES fltk-config PATHS ${FLTK_CONFIG_PATH} ) +ENDIF ( NOT FLTK_CONFIG ) +IF ( FLTK_CONFIG ) + MESSAGE (STATUS " found, using ${FLTK_CONFIG}") + EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --cxxflags --use-gl --use-images OUTPUT_VARIABLE FLTK_CFLAGS - OUTPUT_STRIP_TRAILING_WHITESPACE) - EXECUTE_PROCESS (COMMAND fltk-config --ldflags --use-gl --use-images + OUTPUT_STRIP_TRAILING_WHITESPACE ) + EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --ldflags --use-gl --use-images OUTPUT_VARIABLE FLTK_LDFLAGS - OUTPUT_STRIP_TRAILING_WHITESPACE) - MESSAGE (STATUS "Found FLTK") + OUTPUT_STRIP_TRAILING_WHITESPACE ) SET (FLTK_FOUND TRUE) -ELSE (FLTKCONFIG) - MESSAGE (FATAL_ERROR "FLTK not found, aborting") - SET (FLTK_FOUND FALSE) -ENDIF (FLTKCONFIG) +ELSE ( FLTK_CONFIG ) + MESSAGE ( FATAL_ERROR "Unable to locate fltk-config. FLTK is not properly installed, aborting. If FLTK is installed, please specify full path of fltk-config using the FLTK_CONFIG variable." ) + SET ( FLTK_FOUND FALSE ) +ENDIF ( FLTK_CONFIG ) IF (APPLE) # define flags to avoid the error: Modified: code/stage/trunk/RELEASE.txt =================================================================== --- code/stage/trunk/RELEASE.txt 2008-07-29 20:26:46 UTC (rev 6933) +++ code/stage/trunk/RELEASE.txt 2008-07-29 23:07:08 UTC (rev 6934) @@ -1,5 +1,7 @@ Version 3.0.1 ------------- +This version incorporates a number of fixes made since the previous major release: + * Player plugin * added unit test suite to verify functionality (build instructions in libstageplugin/test directory) @@ -15,8 +17,6 @@ * blobfinder now returns blobs in the correct horizontal order * blobfinder sensor visualization displays in the plane of the screen - - Version 3.0.0 ------------- Stage 3.0.0 consists of these components: This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-30 11:08:07
|
Revision: 6938 http://playerstage.svn.sourceforge.net/playerstage/?rev=6938&view=rev Author: jeremy_asher Date: 2008-07-30 18:08:13 +0000 (Wed, 30 Jul 2008) Log Message: ----------- stage: added a CMake option to build CPack scripts (off by default), fixed some warnings Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstageplugin/p_laser.cc Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-07-30 00:25:51 UTC (rev 6937) +++ code/stage/trunk/CMakeLists.txt 2008-07-30 18:08:13 UTC (rev 6938) @@ -12,6 +12,7 @@ OPTION (BUILD_PLAYER_PLUGIN "Build Player plugin" ON) OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) +OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF) cmake_minimum_required( VERSION 2.4 FATAL_ERROR ) @@ -130,14 +131,16 @@ # generate a cpack config file used to create packaged tarballs -INCLUDE(InstallRequiredSystemLibraries) -SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "${PROJECT_NAME}: A Multiple Robot Simulator") -SET(CPACK_PACKAGE_VENDOR "The Player Project") -#SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/DESCRIPTION.txt") -#SET(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.txt") -#SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/COPYING.txt") -SET(CPACK_PACKAGE_VERSION_MAJOR "${V_MAJOR}") -SET(CPACK_PACKAGE_VERSION_MINOR "${V_MINOR}") -SET(CPACK_PACKAGE_VERSION_PATCH "${V_BUGFIX}") -SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME}-${V_MAJOR}.${V_MINOR}") -INCLUDE(CPack) +IF ( CPACK_CFG ) + INCLUDE(InstallRequiredSystemLibraries) + SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "${PROJECT_NAME}: A Multiple Robot Simulator") + SET(CPACK_PACKAGE_VENDOR "The Player Project") + #SET(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/DESCRIPTION.txt") + #SET(CPACK_RESOURCE_FILE_README "${CMAKE_CURRENT_SOURCE_DIR}/README.txt") + #SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/COPYING.txt") + SET(CPACK_PACKAGE_VERSION_MAJOR "${V_MAJOR}") + SET(CPACK_PACKAGE_VERSION_MINOR "${V_MINOR}") + SET(CPACK_PACKAGE_VERSION_PATCH "${V_BUGFIX}") + SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME}-${V_MAJOR}.${V_MINOR}") + INCLUDE(CPack) +ENDIF ( CPACK_CFG ) Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-07-30 00:25:51 UTC (rev 6937) +++ code/stage/trunk/libstage/canvas.cc 2008-07-30 18:08:13 UTC (rev 6938) @@ -741,8 +741,8 @@ clockstr.append( " [ FOLLOW MODE ]" ); int margin = 3; - int width = gl_width( clockstr.c_str() ) + 2 * margin; - int height = gl_height() + 2 * margin; + float width = gl_width( clockstr.c_str() ) + 2 * margin; + float height = gl_height() + 2 * margin; colorstack.Push( 0.8,0.8,1.0 ); // pale blue glRectf( 0, 0, width, height ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-07-30 00:25:51 UTC (rev 6937) +++ code/stage/trunk/libstage/world.cc 2008-07-30 18:08:13 UTC (rev 6938) @@ -227,8 +227,8 @@ (int)(this->interval_sim/thousand) ); if( wf->PropertyExists( entity, "quit_time" ) ) { - this->quit_time = (stg_usec_t)million * - wf->ReadFloat( entity, "quit_time", 0 ); + this->quit_time = (stg_usec_t) ( million * + wf->ReadFloat( entity, "quit_time", 0 ) ); } if( wf->PropertyExists( entity, "resolution" ) ) Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-30 00:25:51 UTC (rev 6937) +++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-30 18:08:13 UTC (rev 6938) @@ -114,10 +114,10 @@ cfg.resolution, cfg.fov, cfg.interval ); cfg.fov = plc->max_angle - plc->min_angle; - cfg.resolution = cfg.fov / ( cfg.sample_count * plc->resolution ); + cfg.resolution = (uint32_t) ( cfg.fov / ( cfg.sample_count * plc->resolution ) ); if ( cfg.resolution < 1 ) cfg.resolution = 1; - cfg.interval = 1.0E6 / plc->scanning_frequency; + cfg.interval = (stg_usec_t) ( 1.0E6 / plc->scanning_frequency ); PRINT_DEBUG3( "setting laser config: resolution %d, fov %.6f, interval %d\n", cfg.resolution, cfg.fov, cfg.interval ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-08-01 11:49:43
|
Revision: 6939 http://playerstage.svn.sourceforge.net/playerstage/?rev=6939&view=rev Author: jeremy_asher Date: 2008-08-01 18:49:44 +0000 (Fri, 01 Aug 2008) Log Message: ----------- stage: major FileManager updates, rgb.txt is now searched for in STAGEPATH as well Modified Paths: -------------- code/stage/trunk/RELEASE.txt code/stage/trunk/assets/CMakeLists.txt code/stage/trunk/config.h.in code/stage/trunk/docsrc/Makefile code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/file_manager.cc code/stage/trunk/libstage/file_manager.hh code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Removed Paths: ------------- code/stage/trunk/.cvsignore code/stage/trunk/docsrc/.cvsignore code/stage/trunk/examples/.cvsignore code/stage/trunk/examples/libplayerc/.cvsignore code/stage/trunk/examples/libstage/.cvsignore code/stage/trunk/libstage/.cvsignore code/stage/trunk/libstageplugin/.cvsignore code/stage/trunk/replace/.cvsignore code/stage/trunk/worlds/.cvsignore code/stage/trunk/worlds/bitmaps/.cvsignore code/stage/trunk/worlds/wifi/.cvsignore Deleted: code/stage/trunk/.cvsignore =================================================================== --- code/stage/trunk/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,24 +0,0 @@ -.DS_Store -.gdb_history -Makefile -Makefile.in -aclocal.m4 -autom4te.cache -config.cache -config.guess -config.h -config.h.in -config.log -config.status -config.sub -configure -depcomp -install-sh -libtool -ltmain.sh -missing -mkinstalldirs -stage.pc -stamp-h -stamp-h.in -stamp-h1 \ No newline at end of file Modified: code/stage/trunk/RELEASE.txt =================================================================== --- code/stage/trunk/RELEASE.txt 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/RELEASE.txt 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,6 +1,7 @@ Version 3.0.1 ------------- -This version incorporates a number of fixes made since the previous major release: +This version incorporates a number of fixes made since the previous +major release: * Player plugin * added unit test suite to verify functionality (build instructions Modified: code/stage/trunk/assets/CMakeLists.txt =================================================================== --- code/stage/trunk/assets/CMakeLists.txt 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/assets/CMakeLists.txt 2008-08-01 18:49:44 UTC (rev 6939) @@ -6,6 +6,6 @@ ) INSTALL(FILES rgb.txt - DESTINATION share/stage/ + DESTINATION share/stage ) Modified: code/stage/trunk/config.h.in =================================================================== --- code/stage/trunk/config.h.in 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/config.h.in 2008-08-01 18:49:44 UTC (rev 6939) @@ -4,7 +4,6 @@ #define PROJECT "@PROJECT_NAME@" #define VERSION "@VERSION@" #define APIVERSION "@APIVERSION@" -#define RGBFILE "@RGBFILE@" #define INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@" #endif Deleted: code/stage/trunk/docsrc/.cvsignore =================================================================== --- code/stage/trunk/docsrc/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/docsrc/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,7 +0,0 @@ -Makefile -Makefile.in -stage_user -stage_reference -player.tag -header.html -stage Modified: code/stage/trunk/docsrc/Makefile =================================================================== --- code/stage/trunk/docsrc/Makefile 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/docsrc/Makefile 2008-08-01 18:49:44 UTC (rev 6939) @@ -16,6 +16,7 @@ ./sourcedocs.sh > sourced.txt doxygen stage.dox cp stage_button.png stage + rm -f sourced.txt # install the latest docs doc-install: doc Deleted: code/stage/trunk/examples/.cvsignore =================================================================== --- code/stage/trunk/examples/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/examples/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,2 +0,0 @@ -Makefile.in -Makefile \ No newline at end of file Deleted: code/stage/trunk/examples/libplayerc/.cvsignore =================================================================== --- code/stage/trunk/examples/libplayerc/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/examples/libplayerc/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,4 +0,0 @@ -Makefile.in -Makefile -ptest -audio Deleted: code/stage/trunk/examples/libstage/.cvsignore =================================================================== --- code/stage/trunk/examples/libstage/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/examples/libstage/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,3 +0,0 @@ -Makefile.in -Makefile -stest Deleted: code/stage/trunk/libstage/.cvsignore =================================================================== --- code/stage/trunk/libstage/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,4 +0,0 @@ -Makefile -Makefile.in -stage -.DS_Store Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/canvas.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -950,7 +950,7 @@ //TODO find a better home for loading textures if( loaded_texture == false ) { - std::string fullpath = world->fileMan->fullPath( "stall.png" ); + std::string fullpath = FileManager::findFile( "assets/stall.png" ); if ( fullpath == "" ) { PRINT_DEBUG( "Unable to load texture.\n" ); } Modified: code/stage/trunk/libstage/file_manager.cc =================================================================== --- code/stage/trunk/libstage/file_manager.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/file_manager.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,3 +1,5 @@ +//#define DEBUG + #include "file_manager.hh" #include "stage.hh" // to get PRINT_DEBUG #include "config.h" // to get INSTALL_PREFIX @@ -2,43 +4,62 @@ +#include <sstream> +#include <fstream> + +std::string searchDirs( const std::vector<std::string> dirs, const std::string filename ) { + for ( unsigned int i=0; i<dirs.size(); i++ ) { + std::string path = dirs[i] + '/' + filename; + PRINT_DEBUG1("FileManager: trying %s\n", path.c_str()); + if ( Stg::FileManager::readable( path ) ) { + return path; + } + } + + PRINT_DEBUG1("FileManager: %s not found.\n", filename.c_str() ); + return ""; +} + namespace Stg { - FileManager::FileManager() { - char *tmp; - - SharePath = INSTALL_PREFIX "/share/stage"; - AssetPath = SharePath + '/' + "assets"; - WorldsRoot = "."; - - paths.push_back( "." ); - paths.push_back( SharePath ); - paths.push_back( AssetPath ); - if( tmp = getenv("STAGEPATH") ) - paths.push_back( tmp ); + FileManager::FileManager() : WorldsRoot( "." ) + { } + + std::string FileManager::stagePath() { + static char* stgPath = getenv("STAGEPATH"); + if ( stgPath == NULL ) + return ""; + else + return std::string( stgPath ); } - std::string FileManager::fullPath( std::string filename ) { + std::string FileManager::findFile( const std::string filename ) { PRINT_DEBUG1("FileManager: trying %s\n", filename.c_str()); if ( readable( filename ) ) return filename; - - for ( unsigned int i=0; i<paths.size(); i++ ) { - std::string path = paths[i] + '/' + filename; - PRINT_DEBUG1("FileManager: trying %s\n", path.c_str()); - if ( readable( path ) ) { - return path; + + static std::vector<std::string> paths; + static bool ranOnce = false; + + // initialize the path list, if necessary + if ( !ranOnce ) { + std::string SharePath = INSTALL_PREFIX "/share/stage"; + paths.push_back( SharePath ); + + std::string stgPath = stagePath(); + + std::istringstream is( stgPath ); + std::string path; + while ( getline( is, path, ':' ) ) { + paths.push_back( path ); + PRINT_DEBUG1("FileManager - INIT: added path %s\n", path.c_str() ); } + + ranOnce = true; + + PRINT_DEBUG1("FileManager - INIT: %d paths in search paths\n", paths.size() ); } - - PRINT_DEBUG("FileManager: Not found.\n"); - return ""; + + // search the path list + return searchDirs( paths, filename ); } - - /*std::string FileManager::fullPathImage( std::string filename ) { - std::string path = ImgPath + '/' + filename; - if ( readable ( path ) ) - return path; - else - return ""; - }*/ - + bool FileManager::readable( std::string path ) { @@ -54,7 +75,7 @@ return false; } } - + std::string FileManager::stripFilename( std::string path ) { std::string pathChars( "\\/" ); size_t loc = path.find_last_of( pathChars ); @@ -63,6 +84,6 @@ else return path.substr( 0, loc ); } - -}; // namespace Stg +}; // namespace Stg + Modified: code/stage/trunk/libstage/file_manager.hh =================================================================== --- code/stage/trunk/libstage/file_manager.hh 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/file_manager.hh 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,7 +1,6 @@ #ifndef _FILE_MANAGER_HH_ #define _FILE_MANAGER_HH_ -#include <fstream> #include <string> #include <vector> @@ -9,24 +8,30 @@ class FileManager { private: - std::vector<std::string> paths; - std::string SharePath; - std::string AssetPath; - std::string CtrlPath; std::string WorldsRoot; - - std::string stripFilename( std::string path ); + + std::string stripFilename( const std::string path ); public: FileManager(); - - std::string fullPath( std::string filename ); - //std::string fullPathImage( std::string filename ); - + + /// Return the path where the current worldfile was loaded from inline const std::string worldsRoot() const { return WorldsRoot; } - inline void newWorld( std::string worldfile ) { + /// Update the worldfile path + inline void newWorld( const std::string worldfile ) { WorldsRoot = stripFilename( worldfile ); } - - bool readable( std::string path ); + + /// Determine whether a file can be opened for reading + static bool readable( const std::string path ); + + /** Search for a file in the current directory, in the + * prefix/share/stage location, and in the locations specified by + * the STAGEPATH environment variable. Returns the first match or + * the original filename if not found. + **/ + static std::string findFile( const std::string filename ); + + /// Return the STAGEPATH environment variable + static std::string stagePath(); }; }; Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/model_load.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -8,8 +8,9 @@ #include <ltdl.h> #include "stage_internal.hh" +#include "file_manager.hh" -#define DEBUG 1 +//#define DEBUG void StgModel::Load() { @@ -379,12 +380,8 @@ int errors = lt_dlinit(); assert(errors==0); - char* stagepath = getenv("STAGEPATH"); - if( stagepath == NULL ) - stagepath = (char*)"."; + lt_dlsetsearchpath( FileManager::stagePath().c_str() ); - lt_dlsetsearchpath( stagepath ); - lt_dlhandle handle = NULL; if(( handle = lt_dlopenext( lib ) )) Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/stage.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -18,6 +18,7 @@ #include "stage_internal.hh" #include "config.h" // results of cmake's system configuration tests +#include "file_manager.hh" static bool init_called = false; @@ -193,24 +194,9 @@ if( file == NULL ) { - const char* searchfiles[] = { - "./rgb.txt", -#ifdef RGBFILE - RGBFILE, -#endif - "../rgb.txt", - NULL }; - - for( int i=0; - searchfiles[i]; - i++ ) - { - const char* filename = searchfiles[i]; - PRINT_DEBUG1( "Attempting to open \"%s\"", filename ); - if( (file = fopen( filename, "r")) ) - break; // opened a file ok - jump out of for loop - } - + std::string rgbFile = FileManager::findFile( "rgb.txt" ); + file = fopen( rgbFile.c_str(), "r" ); + if( file == NULL ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/stage.hh 2008-08-01 18:49:44 UTC (rev 6939) @@ -67,6 +67,7 @@ #endif #include "option.hh" +#include "file_manager.hh" /** The Stage library uses its own namespace */ namespace Stg @@ -77,7 +78,6 @@ class StgWorld; class StgWorldGui; class StgModel; - class FileManager; class OptionsDlg; class StgCamera; @@ -1065,7 +1065,7 @@ long unsigned int updates; ///< the number of simulated time steps executed so far - FileManager* fileMan; + FileManager fileMan; public: static const int DEFAULT_PPM = 50; // default resolution in pixels per meter Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/world.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -153,8 +153,6 @@ bzero( &this->extent, sizeof(this->extent)); this->real_time_now = 0; - - fileMan = new FileManager(); } StgWorld::~StgWorld( void ) @@ -170,8 +168,6 @@ g_free( token ); world_list = g_list_remove( world_list, this ); - - delete fileMan; } Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstage/worldgui.cc 2008-08-01 18:49:44 UTC (rev 6939) @@ -216,7 +216,7 @@ { PRINT_DEBUG1( "%s.Load()", token ); - fileMan->newWorld( filename ); + fileMan.newWorld( filename ); StgWorld::Load( filename ); @@ -403,7 +403,7 @@ //bool success; const char* pattern = "World Files (*.world)"; - std::string worldsPath( worldGui->fileMan->worldsRoot() ); + std::string worldsPath = worldGui->fileMan.worldsRoot(); worldsPath.append( "/" ); Fl_File_Chooser fc( worldsPath.c_str(), pattern, Fl_File_Chooser::CREATE, "Load World File..." ); fc.ok_label( "Load" ); @@ -415,7 +415,7 @@ filename = fc.value(); if (filename != NULL) { // chose something - if ( worldGui->fileMan->readable( filename ) ) { + if ( FileManager::readable( filename ) ) { // file is readable, clear and load // if (initialized) { @@ -562,7 +562,7 @@ std::string fullpath; - fullpath = worldGui->fileMan->fullPath( "stagelogo.png" ); + fullpath = FileManager::findFile( "assets/stagelogo.png" ); Fl_PNG_Image* png = new Fl_PNG_Image( fullpath.c_str() ); // load image into ram box->image( png ); // attach image to box Deleted: code/stage/trunk/libstageplugin/.cvsignore =================================================================== --- code/stage/trunk/libstageplugin/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/libstageplugin/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,3 +0,0 @@ -Makefile -Makefile.in -.DS_Store Deleted: code/stage/trunk/replace/.cvsignore =================================================================== --- code/stage/trunk/replace/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/replace/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,2 +0,0 @@ -Makefile.in -Makefile Deleted: code/stage/trunk/worlds/.cvsignore =================================================================== --- code/stage/trunk/worlds/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/worlds/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,2 +0,0 @@ -Makefile -Makefile.in Deleted: code/stage/trunk/worlds/bitmaps/.cvsignore =================================================================== --- code/stage/trunk/worlds/bitmaps/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/worlds/bitmaps/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,2 +0,0 @@ -Makefile -Makefile.in \ No newline at end of file Deleted: code/stage/trunk/worlds/wifi/.cvsignore =================================================================== --- code/stage/trunk/worlds/wifi/.cvsignore 2008-07-30 18:08:13 UTC (rev 6938) +++ code/stage/trunk/worlds/wifi/.cvsignore 2008-08-01 18:49:44 UTC (rev 6939) @@ -1,2 +0,0 @@ -Makefile -Makefile.in This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-08-01 12:04:44
|
Revision: 6940 http://playerstage.svn.sourceforge.net/playerstage/?rev=6940&view=rev Author: jeremy_asher Date: 2008-08-01 19:04:51 +0000 (Fri, 01 Aug 2008) Log Message: ----------- stage: improved color database error Modified Paths: -------------- code/stage/trunk/libstage/stage.cc Property Changed: ---------------- code/stage/trunk/ Property changes on: code/stage/trunk ___________________________________________________________________ Modified: svn:ignore - *.cmake .DS_Store .gdb_history Makefile config.h config.h.in libtool ltmain.sh missing mkinstalldirs stage.pc stage.cpack CMakeScripts CMakeFiles install_manifest.txt CMakeCache.txt package build Testing + *.cmake .DS_Store .gdb_history .project .cproject Makefile config.h libtool ltmain.sh missing mkinstalldirs stage.pc stage.cpack CMakeScripts CMakeFiles install_manifest.txt CMakeCache.txt package build Testing Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-08-01 18:49:44 UTC (rev 6939) +++ code/stage/trunk/libstage/stage.cc 2008-08-01 19:04:51 UTC (rev 6940) @@ -200,10 +200,9 @@ if( file == NULL ) { - PRINT_ERR1("unable to open color database: %s", - strerror(errno)); - // Can't fclose(NULL) - //fclose(file); + PRINT_ERR1("unable to open color database: %s " + "(try adding rgb.txt's location to your STAGEPATH)", + strerror(errno)); exit(0); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-09-16 15:34:52
|
Revision: 7023 http://playerstage.svn.sourceforge.net/playerstage/?rev=7023&view=rev Author: rtv Date: 2008-09-16 22:35:01 +0000 (Tue, 16 Sep 2008) Log Message: ----------- fixed bug #2087686 using Toby's patch Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/worlds/benchmark/cave.world code/stage/trunk/worlds/benchmark/hospital.world code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-09-16 22:35:01 UTC (rev 7023) @@ -8,6 +8,8 @@ message( STATUS " found" ) ENDIF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND") +message( STATUS "TODO: Check for fltk_images library" ) + include_directories( ${OPENGL_INCLUDE_DIR} ${LTDL_INCLUDE_DIR} Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/libstage/model.cc 2008-09-16 22:35:01 UTC (rev 7023) @@ -197,6 +197,7 @@ this->rebuild_displaylist = true; this->say_string = NULL; this->subs = 0; + this->used = false; this->stall = false; if( world->IsGUI() ) @@ -1827,6 +1828,32 @@ return NULL; } +StgModel* StgModel::GetUnusedModelOfType( stg_model_type_t type ) +{ + printf( "searching for type %d in model %s type %d\n", type, token, this->type ); + + if( (this->type == type) && (!this->used ) ) + { + this->used = true; + return this; + } + + // this model is no use. try children recursively + for( GList* it = children; it; it=it->next ) + { + StgModel* child = (StgModel*)it->data; + + StgModel* found = child->GetUnusedModelOfType( type ); + if( found ) + return found; + } + + // nothing matching below this model + return NULL; +} + + + StgModel* StgModel::GetModel( const char* modelname ) { // construct the full model name and look it up Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/libstage/stage.hh 2008-09-16 22:35:01 UTC (rev 7023) @@ -1197,6 +1197,7 @@ GHashTable* callbacks; int subs; //< the number of subscriptions to this model + bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() stg_usec_t interval; //< time between updates in us stg_usec_t last_update; //< time of last update in us @@ -1646,6 +1647,10 @@ and has the type indicated by the string */ StgModel* GetUnsubscribedModelOfType( stg_model_type_t type ); + /** returns the first descendent of this model that is unused + and has the type indicated by the string. This model is tagged as used. */ + StgModel* GetUnusedModelOfType( stg_model_type_t type ); + // Iff true, model may output some debugging visualizations and other info //bool debug; Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/libstageplugin/p_driver.cc 2008-09-16 22:35:01 UTC (rev 7023) @@ -437,7 +437,7 @@ // type (i.e. has the right initialization function) and has not // been used before //return( model_match( base_model, addr, typestr, this->devices ) ); - return( base_model->GetUnsubscribedModelOfType( type ) ); + return( base_model->GetUnusedModelOfType( type ) ); } //////////////////////////////////////////////////////////////////////////////// Modified: code/stage/trunk/worlds/benchmark/cave.world =================================================================== --- code/stage/trunk/worlds/benchmark/cave.world 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/worlds/benchmark/cave.world 2008-09-16 22:35:01 UTC (rev 7023) @@ -8,7 +8,7 @@ resolution 0.02 # resolution of the underlying raytrace mode interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 100 # real-time interval between simulation updates in milliseconds paused 1 # configure the GUI window @@ -18,7 +18,7 @@ center [0.068 -0.129] rotate [ 0 0 ] scale 38.592 - interval 10 + interval 50 ) floorplan @@ -29,14 +29,14 @@ bitmap "../bitmaps/cave.png" ) -define rob pioneer2dx -#define rob fancypioneer2dx # alternative with more complex polygons +#define rob pioneer2dx +define rob fancypioneer2dx # alternative with more complex polygons ( origin [ 0 0 0 0 ] sicklaser( samples 180 ) # alternative laser with more complex polygons - # fancysicklaser( samples 180 ) + fancysicklaser( samples 180 ) ranger( pose [ 0 0 -0.050 0 ] ) Modified: code/stage/trunk/worlds/benchmark/hospital.world =================================================================== --- code/stage/trunk/worlds/benchmark/hospital.world 2008-09-16 22:08:26 UTC (rev 7022) +++ code/stage/trunk/worlds/benchmark/hospital.world 2008-09-16 22:35:01 UTC (rev 7023) @@ -18,27 +18,32 @@ # configure the GUI window window ( - size [ 640 480 ] - center [929.990 -377.040] - rotate [ 0.000 0.000 ] - scale 20.806 + size [ 1020.000 724.000 ] + center [-24.574 -17.333] + rotate [ 38.000 -36.500 ] + scale 12.611 + +# size [ 640 480 ] +# center [929.990 -377.040] +# rotate [ 0.000 0.000 ] +# scale 20.806 show_data 0 ) # load an environment bitmap floorplan ( - size [140 60 0.5] - pose [0.000 0.5 0.000 0] + size [140.000 60.000 0.500] + pose [0 0.500 0 0] bitmap "../bitmaps/hospital.png" ) define swarmbot position ( - size [0.1 0.1 0.1] + size [0.100 0.100 0.100] color "red" - ranger( pose [ 0 0 -0.01 0 ] + ranger( pose [ 0 0 -0.010 0 ] scount 12 spose[0] [0 0 0] @@ -1060,1003 +1065,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 "r1361" pose [ 18.000 -14.750 0 0 ] ) -swarmbot( name "r1362" pose [ 18.000 -14.500 0 0 ] ) -swarmbot( name "r1363" pose [ 18.000 -14.250 0 0 ] ) -swarmbot( name "r1364" pose [ 18.000 -14.000 0 0 ] ) -swarmbot( name "r1365" pose [ 18.250 -15.000 0 0 ] ) -swarmbot( name "r1366" pose [ 18.250 -14.750 0 0 ] ) -swarmbot( name "r1367" pose [ 18.250 -14.500 0 0 ] ) -swarmbot( name "r1368" pose [ 18.250 -14.250 0 0 ] ) -swarmbot( name "r1369" pose [ 18.250 -14.000 0 0 ] ) -swarmbot( name "r1370" pose [ 18.500 -15.000 0 0 ] ) -swarmbot( name "r1371" pose [ 18.500 -14.750 0 0 ] ) -swarmbot( name "r1372" pose [ 18.500 -14.500 0 0 ] ) -swarmbot( name "r1373" pose [ 18.500 -14.250 0 0 ] ) -swarmbot( name "r1374" pose [ 18.500 -14.000 0 0 ] ) -swarmbot( name "r1375" pose [ 18.750 -15.000 0 0 ] ) -swarmbot( name "r1376" pose [ 18.750 -14.750 0 0 ] ) -swarmbot( name "r1377" pose [ 18.750 -14.500 0 0 ] ) -swarmbot( name "r1378" pose [ 18.750 -14.250 0 0 ] ) -swarmbot( name "r1379" pose [ 18.750 -14.000 0 0 ] ) -swarmbot( name "r1380" pose [ 19.000 -15.000 0 0 ] ) -swarmbot( name "r1381" pose [ 19.000 -14.750 0 0 ] ) -swarmbot( name "r1382" pose [ 19.000 -14.500 0 0 ] ) -swarmbot( name "r1383" pose [ 19.000 -14.250 0 0 ] ) -swarmbot( name "r1384" pose [ 19.000 -14.000 0 0 ] ) -swarmbot( name "r1385" pose [ 19.250 -15.000 0 0 ] ) -swarmbot( name "r1386" pose [ 19.250 -14.750 0 0 ] ) -swarmbot( name "r1387" pose [ 19.250 -14.500 0 0 ] ) -swarmbot( name "r1388" pose [ 19.250 -14.250 0 0 ] ) -swarmbot( name "r1389" pose [ 19.250 -14.000 0 0 ] ) -swarmbot( name "r1390" pose [ 19.500 -15.000 0 0 ] ) -swarmbot( name "r1391" pose [ 19.500 -14.750 0 0 ] ) -swarmbot( name "r1392" pose [ 19.500 -14.500 0 0 ] ) -swarmbot( name "r1393" pose [ 19.500 -14.250 0 0 ] ) -swarmbot( name "r1394" pose [ 19.500 -14.000 0 0 ] ) -swarmbot( name "r1395" pose [ 19.750 -15.000 0 0 ] ) -swarmbot( name "r1396" pose [ 19.750 -14.750 0 0 ] ) -swarmbot( name "r1397" pose [ 19.750 -14.500 0 0 ] ) -swarmbot( name "r1398" pose [ 19.750 -14.250 0 0 ] ) -swarmbot( name "r1399" pose [ 19.750 -14.000 0 0 ] ) -swarmbot( name "r1400" pose [ 20.000 -15.000 0 0 ] ) -swarmbot( name "r1401" pose [ 20.000 -14.750 0 0 ] ) -swarmbot( name "r1402" pose [ 20.000 -14.500 0 0 ] ) -swarmbot( name "r1403" pose [ 20.000 -14.250 0 0 ] ) -swarmbot( name "r1404" pose [ 20.000 -14.000 0 0 ] ) -swarmbot( name "r1405" pose [ 20.250 -15.000 0 0 ] ) -swarmbot( name "r1406" pose [ 20.250 -14.750 0 0 ] ) -swarmbot( name "r1407" pose [ 20.250 -14.500 0 0 ] ) -swarmbot( name "r1408" pose [ 20.250 -14.250 0 0 ] ) -swarmbot( name "r1409" pose [ 20.250 -14.000 0 0 ] ) -swarmbot( name "r1410" pose [ 20.500 -15.000 0 0 ] ) -swarmbot( name "r1411" pose [ 20.500 -14.750 0 0 ] ) -swarmbot( name "r1412" pose [ 20.500 -14.500 0 0 ] ) -swarmbot( name "r1413" pose [ 20.500 -14.250 0 0 ] ) -swarmbot( name "r1414" pose [ 20.500 -14.000 0 0 ] ) -swarmbot( name "r1415" pose [ 20.750 -15.000 0 0 ] ) -swarmbot( name "r1416" pose [ 20.750 -14.750 0 0 ] ) -swarmbot( name "r1417" pose [ 20.750 -14.500 0 0 ] ) -swarmbot( name "r1418" pose [ 20.750 -14.250 0 0 ] ) -swarmbot( name "r1419" pose [ 20.750 -14.000 0 0 ] ) -swarmbot( name "r1420" pose [ 21.000 -15.000 0 0 ] ) -swarmbot( name "r1421" pose [ 21.000 -14.750 0 0 ] ) -swarmbot( name "r1422" pose [ 21.000 -14.500 0 0 ] ) -swarmbot( name "r1423" pose [ 21.000 -14.250 0 0 ] ) -swarmbot( name "r1424" pose [ 21.000 -14.000 0 0 ] ) -swarmbot( name "r1425" pose [ 21.250 -15.000 0 0 ] ) -swarmbot( name "r1426" pose [ 21.250 -14.750 0 0 ] ) -swarmbot( name "r1427" pose [ 21.250 -14.500 0 0 ] ) -swarmbot( name "r1428" pose [ 21.250 -14.250 0 0 ] ) -swarmbot( name "r1429" pose [ 21.250 -14.000 0 0 ] ) -swarmbot( name "r1430" pose [ 21.500 -15.000 0 0 ] ) -swarmbot( name "r1431" pose [ 21.500 -14.750 0 0 ] ) -swarmbot( name "r1432" pose [ 21.500 -14.500 0 0 ] ) -swarmbot( name "r1433" pose [ 21.500 -14.250 0 0 ] ) -swarmbot( name "r1434" pose [ 21.500 -14.000 0 0 ] ) -swarmbot( name "r1435" pose [ 21.750 -15.000 0 0 ] ) -swarmbot( name "r1436" pose [ 21.750 -14.750 0 0 ] ) -swarmbot( name "r1437" pose [ 21.750 -14.500 0 0 ] ) -swarmbot( name "r1438" pose [ 21.750 -14.250 0 0 ] ) -swarmbot( name "r1439" pose [ 21.750 -14.000 0 0 ] ) -swarmbot( name "r1440" pose [ 22.000 -15.000 0 0 ] ) -swarmbot( name "r1441" pose [ 22.000 -14.750 0 0 ] ) -swarmbot( name "r1442" pose [ 22.000 -14.500 0 0 ] ) -swarmbot( name "r1443" pose [ 22.000 -14.250 0 0 ] ) -swarmbot( name "r1444" pose [ 22.000 -14.000 0 0 ] ) -swarmbot( name "r1445" pose [ 22.250 -15.000 0 0 ] ) -swarmbot( name "r1446" pose [ 22.250 -14.750 0 0 ] ) -swarmbot( name "r1447" pose [ 22.250 -14.500 0 0 ] ) -swarmbot( name "r1448" pose [ 22.250 -14.250 0 0 ] ) -swarmbot( name "r1449" pose [ 22.250 -14.000 0 0 ] ) -swarmbot( name "r1450" pose [ 22.500 -15.000 0 0 ] ) -swarmbot( name "r1451" pose [ 22.500 -14.750 0 0 ] ) -swarmbot( name "r1452" pose [ 22.500 -14.500 0 0 ] ) -swarmbot( name "r1453" pose [ 22.500 -14.250 0 0 ] ) -swarmbot( name "r1454" pose [ 22.500 -14.000 0 0 ] ) -swarmbot( name "r1455" pose [ 22.750 -15.000 0 0 ] ) -swarmbot( name "r1456" pose [ 22.750 -14.750 0 0 ] ) -swarmbot( name "r1457" pose [ 22.750 -14.500 0 0 ] ) -swarmbot( name "r1458" pose [ 22.750 -14.250 0 0 ] ) -swarmbot( name "r1459" pose [ 22.750 -14.000 0 0 ] ) -swarmbot( name "r1460" pose [ 23.000 -15.000 0 0 ] ) -swarmbot( name "r1461" pose [ 23.000 -14.750 0 0 ] ) -swarmbot( name "r1462" pose [ 23.000 -14.500 0 0 ] ) -swarmbot( name "r1463" pose [ 23.000 -14.250 0 0 ] ) -swarmbot( name "r1464" pose [ 23.000 -14.000 0 0 ] ) -swarmbot( name "r1465" pose [ 23.250 -15.000 0 0 ] ) -swarmbot( name "r1466" pose [ 23.250 -14.750 0 0 ] ) -swarmbot( name "r1467" pose [ 23.250 -14.500 0 0 ] ) -swarmbot( name "r1468" pose [ 23.250 -14.250 0 0 ] ) -swarmbot( name "r1469" pose [ 23.250 -14.000 0 0 ] ) -swarmbot( name "r1470" pose [ 23.500 -15.000 0 0 ] ) -swarmbot( name "r1471" pose [ 23.500 -14.750 0 0 ] ) -swarmbot( name "r1472" pose [ 23.500 -14.500 0 0 ] ) -swarmbot( name "r1473" pose [ 23.500 -14.250 0 0 ] ) -swarmbot( name "r1474" pose [ 23.500 -14.000 0 0 ] ) -swarmbot( name "r1475" pose [ 23.750 -15.000 0 0 ] ) -swarmbot( name "r1476" pose [ 23.750 -14.750 0 0 ] ) -swarmbot( name "r1477" pose [ 23.750 -14.500 0 0 ] ) -swarmbot( name "r1478" pose [ 23.750 -14.250 0 0 ] ) -swarmbot( name "r1479" pose [ 23.750 -14.000 0 0 ] ) -swarmbot( name "r1480" pose [ 24.000 -15.000 0 0 ] ) -swarmbot( name "r1481" pose [ 24.000 -14.750 0 0 ] ) -swarmbot( name "r1482" pose [ 24.000 -14.500 0 0 ] ) -swarmbot( name "r1483" pose [ 24.000 -14.250 0 0 ] ) -swarmbot( name "r1484" pose [ 24.000 -14.000 0 0 ] ) -swarmbot( name "r1485" pose [ 24.250 -15.000 0 0 ] ) -swarmbot( name "r1486" pose [ 24.250 -14.750 0 0 ] ) -swarmbot( name "r1487" pose [ 24.250 -14.500 0 0 ] ) -swarmbot( name "r1488" pose [ 24.250 -14.250 0 0 ] ) -swarmbot( name "r1489" pose [ 24.250 -14.000 0 0 ] ) -swarmbot( name "r1490" pose [ 24.500 -15.000 0 0 ] ) -swarmbot( name "r1491" pose [ 24.500 -14.750 0 0 ] ) -swarmbot( name "r1492" pose [ 24.500 -14.500 0 0 ] ) -swarmbot( name "r1493" pose [ 24.500 -14.250 0 0 ] ) -swarmbot( name "r1494" pose [ 24.500 -14.000 0 0 ] ) -swarmbot( name "r1495" pose [ 24.750 -15.000 0 0 ] ) -swarmbot( name "r1496" pose [ 24.750 -14.750 0 0 ] ) -swarmbot( name "r1497" pose [ 24.750 -14.500 0 0 ] ) -swarmbot( name "r1498" pose [ 24.750 -14.250 0 0 ] ) -swarmbot( name "r1499" pose [ 24.750 -14.000 0 0 ] ) -swarmbot( name "r1500" pose [ 25.000 -15.000 0 0 ] ) -swarmbot( name "r1501" pose [ 25.000 -14.750 0 0 ] ) -swarmbot( name "r1502" pose [ 25.000 -14.500 0 0 ] ) -swarmbot( name "r1503" pose [ 25.000 -14.250 0 0 ] ) -swarmbot( name "r1504" pose [ 25.000 -14.000 0 0 ] ) -swarmbot( name "r1505" pose [ 25.250 -15.000 0 0 ] ) -swarmbot( name "r1506" pose [ 25.250 -14.750 0 0 ] ) -swarmbot( name "r1507" pose [ 25.250 -14.500 0 0 ] ) -swarmbot( name "r1508" pose [ 25.250 -14.250 0 0 ] ) -swarmbot( name "r1509" pose [ 25.250 -14.000 0 0 ] ) -swarmbot( name "r1510" pose [ 25.500 -15.000 0 0 ] ) -swarmbot( name "r1511" pose [ 25.500 -14.750 0 0 ] ) -swarmbot( name "r1512" pose [ 25.500 -14.500 0 0 ] ) -swarmbot( name "r1513" pose [ 25.500 -14.250 0 0 ] ) -swarmbot( name "r1514" pose [ 25.500 -14.000 0 0 ] ) -swarmbot( name "r1515" pose [ 25.750 -15.000 0 0 ] ) -swarmbot( name "r1516" pose [ 25.750 -14.750 0 0 ] ) -swarmbot( name "r1517" pose [ 25.750 -14.500 0 0 ] ) -swarmbot( name "r1518" pose [ 25.750 -14.250 0 0 ] ) -swarmbot( name "r1519" pose [ 25.750 -14.000 0 0 ] ) -swarmbot( name "r1520" pose [ 26.000 -15.000 0 0 ] ) -swarmbot( name "r1521" pose [ 26.000 -14.750 0 0 ] ) -swarmbot( name "r1522" pose [ 26.000 -14.500 0 0 ] ) -swarmbot( name "r1523" pose [ 26.000 -14.250 0 0 ] ) -swarmbot( name "r1524" pose [ 26.000 -14.000 0 0 ] ) -swarmbot( name "r1525" pose [ 26.250 -15.000 0 0 ] ) -swarmbot( name "r1526" pose [ 26.250 -14.750 0 0 ] ) -swarmbot( name "r1527" pose [ 26.250 -14.500 0 0 ] ) -swarmbot( name "r1528" pose [ 26.250 -14.250 0 0 ] ) -swarmbot( name "r1529" pose [ 26.250 -14.000 0 0 ] ) -swarmbot( name "r1530" pose [ 26.500 -15.000 0 0 ] ) -swarmbot( name "r1531" pose [ 26.500 -14.750 0 0 ] ) -swarmbot( name "r1532" pose [ 26.500 -14.500 0 0 ] ) -swarmbot( name "r1533" pose [ 26.500 -14.250 0 0 ] ) -swarmbot( name "r1534" pose [ 26.500 -14.000 0 0 ] ) -swarmbot( name "r1535" pose [ 26.750 -15.000 0 0 ] ) -swarmbot( name "r1536" pose [ 26.750 -14.750 0 0 ] ) -swarmbot( name "r1537" pose [ 26.750 -14.500 0 0 ] ) -swarmbot( name "r1538" pose [ 26.750 -14.250 0 0 ] ) -swarmbot( name "r1539" pose [ 26.750 -14.000 0 0 ] ) -swarmbot( name "r1540" pose [ 27.000 -15.000 0 0 ] ) -swarmbot( name "r1541" pose [ 27.000 -14.750 0 0 ] ) -swarmbot( name "r1542" pose [ 27.000 -14.500 0 0 ] ) -swarmbot( name "r1543" pose [ 27.000 -14.250 0 0 ] ) -swarmbot( name "r1544" pose [ 27.000 -14.000 0 0 ] ) -swarmbot( name "r1545" pose [ 27.250 -15.000 0 0 ] ) -swarmbot( name "r1546" pose [ 27.250 -14.750 0 0 ] ) -swarmbot( name "r1547" pose [ 27.250 -14.500 0 0 ] ) -swarmbot( name "r1548" pose [ 27.250 -14.250 0 0 ] ) -swarmbot( name "r1549" pose [ 27.250 -14.000 0 0 ] ) -swarmbot( name "r1550" pose [ 27.500 -15.000 0 0 ] ) -swarmbot( name "r1551" pose [ 27.500 -14.750 0 0 ] ) -swarmbot( name "r1552" pose [ 27.500 -14.500 0 0 ] ) -swarmbot( name "r1553" pose [ 27.500 -14.250 0 0 ] ) -swarmbot( name "r1554" pose [ 27.500 -14.000 0 0 ] ) -swarmbot( name "r1555" pose [ 27.750 -15.000 0 0 ] ) -swarmbot( name "r1556" pose [ 27.750 -14.750 0 0 ] ) -swarmbot( name "r1557" pose [ 27.750 -14.500 0 0 ] ) -swarmbot( name "r1558" pose [ 27.750 -14.250 0 0 ] ) -swarmbot( name "r1559" pose [ 27.750 -14.000 0 0 ] ) -swarmbot( name "r1560" pose [ 28.000 -15.000 0 0 ] ) -swarmbot( name "r1561" pose [ 28.000 -14.750 0 0 ] ) -swarmbot( name "r1562" pose [ 28.000 -14.500 0 0 ] ) -swarmbot( name "r1563" pose [ 28.000 -14.250 0 0 ] ) -swarmbot( name "r1564" pose [ 28.000 -14.000 0 0 ] ) -swarmbot( name "r1565" pose [ 28.250 -15.000 0 0 ] ) -swarmbot( name "r1566" pose [ 28.250 -14.750 0 0 ] ) -swarmbot( name "r1567" pose [ 28.250 -14.500 0 0 ] ) -swarmbot( name "r1568" pose [ 28.250 -14.250 0 0 ] ) -swarmbot( name "r1569" pose [ 28.250 -14.000 0 0 ] ) -swarmbot( name "r1570" pose [ 28.500 -15.000 0 0 ] ) -swarmbot( name "r1571" pose [ 28.500 -14.750 0 0 ] ) -swarmbot( name "r1572" pose [ 28.500 -14.500 0 0 ] ) -swarmbot( name "r1573" pose [ 28.500 -14.250 0 0 ] ) -swarmbot( name "r1574" pose [ 28.500 -14.000 0 0 ] ) -swarmbot( name "r1575" pose [ 28.750 -15.000 0 0 ] ) -swarmbot( name "r1576" pose [ 28.750 -14.750 0 0 ] ) -swarmbot( name "r1577" pose [ 28.750 -14.500 0 0 ] ) -swarmbot( name "r1578" pose [ 28.750 -14.250 0 0 ] ) -swarmbot( name "r1579" pose [ 28.750 -14.000 0 0 ] ) -swarmbot( name "r1580" pose [ 29.000 -15.000 0 0 ] ) -swarmbot( name "r1581" pose [ 29.000 -14.750 0 0 ] ) -swarmbot( name "r1582" pose [ 29.000 -14.500 0 0 ] ) -swarmbot( name "r1583" pose [ 29.000 -14.250 0 0 ] ) -swarmbot( name "r1584" pose [ 29.000 -14.000 0 0 ] ) -swarmbot( name "r1585" pose [ 29.250 -15.000 0 0 ] ) -swarmbot( name "r1586" pose [ 29.250 -14.750 0 0 ] ) -swarmbot( name "r1587" pose [ 29.250 -14.500 0 0 ] ) -swarmbot( name "r1588" pose [ 29.250 -14.250 0 0 ] ) -swarmbot( name "r1589" pose [ 29.250 -14.000 0 0 ] ) -swarmbot( name "r1590" pose [ 29.500 -15.000 0 0 ] ) -swarmbot( name "r1591" pose [ 29.500 -14.750 0 0 ] ) -swarmbot( name "r1592" pose [ 29.500 -14.500 0 0 ] ) -swarmbot( name "r1593" pose [ 29.500 -14.250 0 0 ] ) -swarmbot( name "r1594" pose [ 29.500 -14.000 0 0 ] ) -swarmbot( name "r1595" pose [ 29.750 -15.000 0 0 ] ) -swarmbot( name "r1596" pose [ 29.750 -14.750 0 0 ] ) -swarmbot( name "r1597" pose [ 29.750 -14.500 0 0 ] ) -swarmbot( name "r1598" pose [ 29.750 -14.250 0 0 ] ) -swarmbot( name "r1599" pose [ 29.750 -14.000 0 0 ] ) -swarmbot( name "r1600" pose [ 30.000 -15.000 0 0 ] ) -swarmbot( name "r1601" pose [ 30.000 -14.750 0 0 ] ) -swarmbot( name "r1602" pose [ 30.000 -14.500 0 0 ] ) -swarmbot( name "r1603" pose [ 30.000 -14.250 0 0 ] ) -swarmbot( name "r1604" pose [ 30.000 -14.000 0 0 ] ) -swarmbot( name "r1605" pose [ 30.250 -15.000 0 0 ] ) -swarmbot( name "r1606" pose [ 30.250 -14.750 0 0 ] ) -swarmbot( name "r1607" pose [ 30.250 -14.500 0 0 ] ) -swarmbot( name "r1608" pose [ 30.250 -14.250 0 0 ] ) -swarmbot( name "r1609" pose [ 30.250 -14.000 0 0 ] ) -swarmbot( name "r1610" pose [ 30.500 -15.000 0 0 ] ) -swarmbot( name "r1611" pose [ 30.500 -14.750 0 0 ] ) -swarmbot( name "r1612" pose [ 30.500 -14.500 0 0 ] ) -swarmbot( name "r1613" pose [ 30.500 -14.250 0 0 ] ) -swarmbot( name "r1614" pose [ 30.500 -14.000 0 0 ] ) -swarmbot( name "r1615" pose [ 30.750 -15.000 0 0 ] ) -swarmbot( name "r1616" pose [ 30.750 -14.750 0 0 ] ) -swarmbot( name "r1617" pose [ 30.750 -14.500 0 0 ] ) -swarmbot( name "r1618" pose [ 30.750 -14.250 0 0 ] ) -swarmbot( name "r1619" pose [ 30.750 -14.000 0 0 ] ) -swarmbot( name "r1620" pose [ 31.000 -15.000 0 0 ] ) -swarmbot( name "r1621" pose [ 31.000 -14.750 0 0 ] ) -swarmbot( name "r1622" pose [ 31.000 -14.500 0 0 ] ) -swarmbot( name "r1623" pose [ 31.000 -14.250 0 0 ] ) -swarmbot( name "r1624" pose [ 31.000 -14.000 0 0 ] ) -swarmbot( name "r1625" pose [ 31.250 -15.000 0 0 ] ) -swarmbot( name "r1626" pose [ 31.250 -14.750 0 0 ] ) -swarmbot( name "r1627" pose [ 31.250 -14.500 0 0 ] ) -swarmbot( name "r1628" pose [ 31.250 -14.250 0 0 ] ) -swarmbot( name "r1629" pose [ 31.250 -14.000 0 0 ] ) -swarmbot( name "r1630" pose [ 31.500 -15.000 0 0 ] ) -swarmbot( name "r1631" pose [ 31.500 -14.750 0 0 ] ) -swarmbot( name "r1632" pose [ 31.500 -14.500 0 0 ] ) -swarmbot( name "r1633" pose [ 31.500 -14.250 0 0 ] ) -swarmbot( name "r1634" pose [ 31.500 -14.000 0 0 ] ) -swarmbot( name "r1635" pose [ 31.750 -15.000 0 0 ] ) -swarmbot( name "r1636" pose [ 31.750 -14.750 0 0 ] ) -swarmbot( name "r1637" pose [ 31.750 -14.500 0 0 ] ) -swarmbot( name "r1638" pose [ 31.750 -14.250 0 0 ] ) -swarmbot( name "r1639" pose [ 31.750 -14.000 0 0 ] ) -swarmbot( name "r1640" pose [ 32.000 -15.000 0 0 ] ) -swarmbot( name "r1641" pose [ 32.000 -14.750 0 0 ] ) -swarmbot( name "r1642" pose [ 32.000 -14.500 0 0 ] ) -swarmbot( name "r1643" pose [ 32.000 -14.250 0 0 ] ) -swarmbot( name "r1644" pose [ 32.000 -14.000 0 0 ] ) -swarmbot( name "r1645" pose [ 32.250 -15.000 0 0 ] ) -swarmbot( name "r1646" pose [ 32.250 -14.750 0 0 ] ) -swarmbot( name "r1647" pose [ 32.250 -14.500 0 0 ] ) -swarmbot( name "r1648" pose [ 32.250 -14.250 0 0 ] ) -swarmbot( name "r1649" pose [ 32.250 -14.000 0 0 ] ) -swarmbot( name "r1650" pose [ 32.500 -15.000 0 0 ] ) -swarmbot( name "r1651" pose [ 32.500 -14.750 0 0 ] ) -swarmbot( name "r1652" pose [ 32.500 -14.500 0 0 ] ) -swarmbot( name "r1653" pose [ 32.500 -14.250 0 0 ] ) -swarmbot( name "r1654" pose [ 32.500 -14.000 0 0 ] ) -swarmbot( name "r1655" pose [ 32.750 -15.000 0 0 ] ) -swarmbot( name "r1656" pose [ 32.750 -14.750 0 0 ] ) -swarmbot( name "r1657" pose [ 32.750 -14.500 0 0 ] ) -swarmbot( name "r1658" pose [ 32.750 -14.250 0 0 ] ) -swarmbot( name "r1659" pose [ 32.750 -14.000 0 0 ] ) -swarmbot( name "r1660" pose [ 33.000 -15.000 0 0 ] ) -swarmbot( name "r1661" pose [ 33.000 -14.750 0 0 ] ) -swarmbot( name "r1662" pose [ 33.000 -14.500 0 0 ] ) -swarmbot( name "r1663" pose [ 33.000 -14.250 0 0 ] ) -swarmbot( name "r1664" pose [ 33.000 -14.000 0 0 ] ) -swarmbot( name "r1665" pose [ 33.250 -15.000 0 0 ] ) -swarmbot( name "r1666" pose [ 33.250 -14.750 0 0 ] ) -swarmbot( name "r1667" pose [ 33.250 -14.500 0 0 ] ) -swarmbot( name "r1668" pose [ 33.250 -14.250 0 0 ] ) -swarmbot( name "r1669" pose [ 33.250 -14.000 0 0 ] ) -swarmbot( name "r1670" pose [ 33.500 -15.000 0 0 ] ) -swarmbot( name "r1671" pose [ 33.500 -14.750 0 0 ] ) -swarmbot( name "r1672" pose [ 33.500 -14.500 0 0 ] ) -swarmbot( name "r1673" pose [ 33.500 -14.250 0 0 ] ) -swarmbot( name "r1674" pose [ 33.500 -14.000 0 0 ] ) -swarmbot( name "r1675" pose [ 33.750 -15.000 0 0 ] ) -swarmbot( name "r1676" pose [ 33.750 -14.750 0 0 ] ) -swarmbot( name "r1677" pose [ 33.750 -14.500 0 0 ] ) -swarmbot( name "r1678" pose [ 33.750 -14.250 0 0 ] ) -swarmbot( name "r1679" pose [ 33.750 -14.000 0 0 ] ) -swarmbot( name "r1680" pose [ 34.000 -15.000 0 0 ] ) -swarmbot( name "r1681" pose [ 34.000 -14.750 0 0 ] ) -swarmbot( name "r1682" pose [ 34.000 -14.500 0 0 ] ) -swarmbot( name "r1683" pose [ 34.000 -14.250 0 0 ] ) -swarmbot( name "r1684" pose [ 34.000 -14.000 0 0 ] ) -swarmbot( name "r1685" pose [ 34.250 -15.000 0 0 ] ) -swarmbot( name "r1686" pose [ 34.250 -14.750 0 0 ] ) -swarmbot( name "r1687" pose [ 34.250 -14.500 0 0 ] ) -swarmbot( name "r1688" pose [ 34.250 -14.250 0 0 ] ) -swarmbot( name "r1689" pose [ 34.250 -14.000 0 0 ] ) -swarmbot( name "r1690" pose [ 34.500 -15.000 0 0 ] ) -swarmbot( name "r1691" pose [ 34.500 -14.750 0 0 ] ) -swarmbot( name "r1692" pose [ 34.500 -14.500 0 0 ] ) -swarmbot( name "r1693" pose [ 34.500 -14.250 0 0 ] ) -swarmbot( name "r1694" pose [ 34.500 -14.000 0 0 ] ) -swarmbot( name "r1695" pose [ 34.750 -15.000 0 0 ] ) -swarmbot( name "r1696" pose [ 34.750 -14.750 0 0 ] ) -swarmbot( name "r1697" pose [ 34.750 -14.500 0 0 ] ) -swarmbot( name "r1698" pose [ 34.750 -14.250 0 0 ] ) -swarmbot( name "r1699" pose [ 34.750 -14.000 0 0 ] ) -swarmbot( name "r1700" pose [ 35.000 -15.000 0 0 ] ) -swarmbot( name "r1701" pose [ 35.000 -14.750 0 0 ] ) -swarmbot( name "r1702" pose [ 35.000 -14.500 0 0 ] ) -swarmbot( name "r1703" pose [ 35.000 -14.250 0 0 ] ) -swarmbot( name "r1704" pose [ 35.000 -14.000 0 0 ] ) -swarmbot( name "r1705" pose [ 35.250 -15.000 0 0 ] ) -swarmbot( name "r1706" pose [ 35.250 -14.750 0 0 ] ) -swarmbot( name "r1707" pose [ 35.250 -14.500 0 0 ] ) -swarmbot( name "r1708" pose [ 35.250 -14.250 0 0 ] ) -swarmbot( name "r1709" pose [ 35.250 -14.000 0 0 ] ) -swarmbot( name "r1710" pose [ 35.500 -15.000 0 0 ] ) -swarmbot( name "r1711" pose [ 35.500 -14.750 0 0 ] ) -swarmbot( name "r1712" pose [ 35.500 -14.500 0 0 ] ) -swarmbot( name "r1713" pose [ 35.500 -14.250 0 0 ] ) -swarmbot( name "r1714" pose [ 35.500 -14.000 0 0 ] ) -swarmbot( name "r1715" pose [ 35.750 -15.000 0 0 ] ) -swarmbot( name "r1716" pose [ 35.750 -14.750 0 0 ] ) -swarmbot( name "r1717" pose [ 35.750 -14.500 0 0 ] ) -swarmbot( name "r1718" pose [ 35.750 -14.250 0 0 ] ) -swarmbot( name "r1719" pose [ 35.750 -14.000 0 0 ] ) -swarmbot( name "r1720" pose [ 36.000 -15.000 0 0 ] ) -swarmbot( name "r1721" pose [ 36.000 -14.750 0 0 ] ) -swarmbot( name "r1722" pose [ 36.000 -14.500 0 0 ] ) -swarmbot( name "r1723" pose [ 36.000 -14.250 0 0 ] ) -swarmbot( name "r1724" pose [ 36.000 -14.000 0 0 ] ) -swarmbot( name "r1725" pose [ 36.250 -15.000 0 0 ] ) -swarmbot( name "r1726" pose [ 36.250 -14.750 0 0 ] ) -swarmbot( name "r1727" pose [ 36.250 -14.500 0 0 ] ) -swarmbot( name "r1728" pose [ 36.250 -14.250 0 0 ] ) -swarmbot( name "r1729" pose [ 36.250 -14.000 0 0 ] ) -swarmbot( name "r1730" pose [ 36.500 -15.000 0 0 ] ) -swarmbot( name "r1731" pose [ 36.500 -14.750 0 0 ] ) -swarmbot( name "r1732" pose [ 36.500 -14.500 0 0 ] ) -swarmbot( name "r1733" pose [ 36.500 -14.250 0 0 ] ) -swarmbot( name "r1734" pose [ 36.500 -14.000 0 0 ] ) -swarmbot( name "r1735" pose [ 36.750 -15.000 0 0 ] ) -swarmbot( name "r1736" pose [ 36.750 -14.750 0 0 ] ) -swarmbot( name "r1737" pose [ 36.750 -14.500 0 0 ] ) -swarmbot( name "r1738" pose [ 36.750 -14.250 0 0 ] ) -swarmbot( name "r1739" pose [ 36.750 -14.000 0 0 ] ) -swarmbot( name "r1740" pose [ 37.000 -15.000 0 0 ] ) -swarmbot( name "r1741" pose [ 37.000 -14.750 0 0 ] ) -swarmbot( name "r1742" pose [ 37.000 -14.500 0 0 ] ) -swarmbot( name "r1743" pose [ 37.000 -14.250 0 0 ] ) -swarmbot( name "r1744" pose [ 37.000 -14.000 0 0 ] ) -swarmbot( name "r1745" pose [ 37.250 -15.000 0 0 ] ) -swarmbot( name "r1746" pose [ 37.250 -14.750 0 0 ] ) -swarmbot( name "r1747" pose [ 37.250 -14.500 0 0 ] ) -swarmbot( name "r1748" pose [ 37.250 -14.250 0 0 ] ) -swarmbot( name "r1749" pose [ 37.250 -14.000 0 0 ] ) -swarmbot( name "r1750" pose [ 37.500 -15.000 0 0 ] ) -swarmbot( name "r1751" pose [ 37.500 -14.750 0 0 ] ) -swarmbot( name "r1752" pose [ 37.500 -14.500 0 0 ] ) -swarmbot( name "r1753" pose [ 37.500 -14.250 0 0 ] ) -swarmbot( name "r1754" pose [ 37.500 -14.000 0 0 ] ) -swarmbot( name "r1755" pose [ 37.750 -15.000 0 0 ] ) -swarmbot( name "r1756" pose [ 37.750 -14.750 0 0 ] ) -swarmbot( name "r1757" pose [ 37.750 -14.500 0 0 ] ) -swarmbot( name "r1758" pose [ 37.750 -14.250 0 0 ] ) -swarmbot( name "r1759" pose [ 37.750 -14.000 0 0 ] ) -swarmbot( name "r1760" pose [ 38.000 -15.000 0 0 ] ) -swarmbot( name "r1761" pose [ 38.000 -14.750 0 0 ] ) -swarmbot( name "r1762" pose [ 38.000 -14.500 0 0 ] ) -swarmbot( name "r1763" pose [ 38.000 -14.250 0 0 ] ) -swarmbot( name "r1764" pose [ 38.000 -14.000 0 0 ] ) -swarmbot( name "r1765" pose [ 38.250 -15.000 0 0 ] ) -swarmbot( name "r1766" pose [ 38.250 -14.750 0 0 ] ) -swarmbot( name "r1767" pose [ 38.250 -14.500 0 0 ] ) -swarmbot( name "r1768" pose [ 38.250 -14.250 0 0 ] ) -swarmbot( name "r1769" pose [ 38.250 -14.000 0 0 ] ) -swarmbot( name "r1770" pose [ 38.500 -15.000 0 0 ] ) -swarmbot( name "r1771" pose [ 38.500 -14.750 0 0 ] ) -swarmbot( name "r1772" pose [ 38.500 -14.500 0 0 ] ) -swarmbot( name "r1773" pose [ 38.500 -14.250 0 0 ] ) -swarmbot( name "r1774" pose [ 38.500 -14.000 0 0 ] ) -swarmbot( name "r1775" pose [ 38.750 -15.000 0 0 ] ) -swarmbot( name "r1776" pose [ 38.750 -14.750 0 0 ] ) -swarmbot( name "r1777" pose [ 38.750 -14.500 0 0 ] ) -swarmbot( name "r1778" pose [ 38.750 -14.250 0 0 ] ) -swarmbot( name "r1779" pose [ 38.750 -14.000 0 0 ] ) -swarmbot( name "r1780" pose [ 39.000 -15.000 0 0 ] ) -swarmbot( name "r1781" pose [ 39.000 -14.750 0 0 ] ) -swarmbot( name "r1782" pose [ 39.000 -14.500 0 0 ] ) -swarmbot( name "r1783" pose [ 39.000 -14.250 0 0 ] ) -swarmbot( name "r1784" pose [ 39.000 -14.000 0 0 ] ) -swarmbot( name "r1785" pose [ 39.250 -15.000 0 0 ] ) -swarmbot( name "r1786" pose [ 39.250 -14.750 0 0 ] ) -swarmbot( name "r1787" pose [ 39.250 -14.500 0 0 ] ) -swarmbot( name "r1788" pose [ 39.250 -14.250 0 0 ] ) -swarmbot( name "r1789" pose [ 39.250 -14.000 0 0 ] ) -swarmbot( name "r1790" pose [ 39.500 -15.000 0 0 ] ) -swarmbot( name "r1791" pose [ 39.500 -14.750 0 0 ] ) -swarmbot( name "r1792" pose [ 39.500 -14.500 0 0 ] ) -swarmbot( name "r1793" pose [ 39.500 -14.250 0 0 ] ) -swarmbot( name "r1794" pose [ 39.500 -14.000 0 0 ] ) -swarmbot( name "r1795" pose [ 39.750 -15.000 0 0 ] ) -swarmbot( name "r1796" pose [ 39.750 -14.750 0 0 ] ) -swarmbot( name "r1797" pose [ 39.750 -14.500 0 0 ] ) -swarmbot( name "r1798" pose [ 39.750 -14.250 0 0 ] ) -swarmbot( name "r1799" pose [ 39.750 -14.000 0 0 ] ) -swarmbot( name "r1800" pose [ 40.000 -15.000 0 0 ] ) -swarmbot( name "r1801" pose [ 40.000 -14.750 0 0 ] ) -swarmbot( name "r1802" pose [ 40.000 -14.500 0 0 ] ) -swarmbot( name "r1803" pose [ 40.000 -14.250 0 0 ] ) -swarmbot( name "r1804" pose [ 40.000 -14.000 0 0 ] ) -swarmbot( name "r1805" pose [ 40.250 -15.000 0 0 ] ) -swarmbot( name "r1806" pose [ 40.250 -14.750 0 0 ] ) -swarmbot( name "r1807" pose [ 40.250 -14.500 0 0 ] ) -swarmbot( name "r1808" pose [ 40.250 -14.250 0 0 ] ) -swarmbot( name "r1809" pose [ 40.250 -14.000 0 0 ] ) -swarmbot( name "r1810" pose [ 40.500 -15.000 0 0 ] ) -swarmbot( name "r1811" pose [ 40.500 -14.750 0 0 ] ) -swarmbot( name "r1812" pose [ 40.500 -14.500 0 0 ] ) -swarmbot( name "r1813" pose [ 40.500 -14.250 0 0 ] ) -swarmbot( name "r1814" pose [ 40.500 -14.000 0 0 ] ) -swarmbot( name "r1815" pose [ 40.750 -15.000 0 0 ] ) -swarmbot( name "r1816" pose [ 40.750 -14.750 0 0 ] ) -swarmbot( name "r1817" pose [ 40.750 -14.500 0 0 ] ) -swarmbot( name "r1818" pose [ 40.750 -14.250 0 0 ] ) -swarmbot( name "r1819" pose [ 40.750 -14.000 0 0 ] ) -swarmbot( name "r1820" pose [ 41.000 -15.000 0 0 ] ) -swarmbot( name "r1821" pose [ 41.000 -14.750 0 0 ] ) -swarmbot( name "r1822" pose [ 41.000 -14.500 0 0 ] ) -swarmbot( name "r1823" pose [ 41.000 -14.250 0 0 ] ) -swarmbot( name "r1824" pose [ 41.000 -14.000 0 0 ] ) -swarmbot( name "r1825" pose [ 41.250 -15.000 0 0 ] ) -swarmbot( name "r1826" pose [ 41.250 -14.750 0 0 ] ) -swarmbot( name "r1827" pose [ 41.250 -14.500 0 0 ] ) -swarmbot( name "r1828" pose [ 41.250 -14.250 0 0 ] ) -swarmbot( name "r1829" pose [ 41.250 -14.000 0 0 ] ) -swarmbot( name "r1830" pose [ 41.500 -15.000 0 0 ] ) -swarmbot( name "r1831" pose [ 41.500 -14.750 0 0 ] ) -swarmbot( name "r1832" pose [ 41.500 -14.500 0 0 ] ) -swarmbot( name "r1833" pose [ 41.500 -14.250 0 0 ] ) -swarmbot( name "r1834" pose [ 41.500 -14.000 0 0 ] ) -swarmbot( name "r1835" pose [ 41.750 -15.000 0 0 ] ) -swarmbot( name "r1836" pose [ 41.750 -14.750 0 0 ] ) -swarmbot( name "r1837" pose [ 41.750 -14.500 0 0 ] ) -swarmbot( name "r1838" pose [ 41.750 -14.250 0 0 ] ) -swarmbot( name "r1839" pose [ 41.750 -14.000 0 0 ] ) -swarmbot( name "r1840" pose [ 42.000 -15.000 0 0 ] ) -swarmbot( name "r1841" pose [ 42.000 -14.750 0 0 ] ) -swarmbot( name "r1842" pose [ 42.000 -14.500 0 0 ] ) -swarmbot( name "r1843" pose [ 42.000 -14.250 0 0 ] ) -swarmbot( name "r1844" pose [ 42.000 -14.000 0 0 ] ) -swarmbot( name "r1845" pose [ 42.250 -15.000 0 0 ] ) -swarmbot( name "r1846" pose [ 42.250 -14.750 0 0 ] ) -swarmbot( name "r1847" pose [ 42.250 -14.500 0 0 ] ) -swarmbot( name "r1848" pose [ 42.250 -14.250 0 0 ] ) -swarmbot( name "r1849" pose [ 42.250 -14.000 0 0 ] ) -swarmbot( name "r1850" pose [ 42.500 -15.000 0 0 ] ) -swarmbot( name "r1851" pose [ 42.500 -14.750 0 0 ] ) -swarmbot( name "r1852" pose [ 42.500 -14.500 0 0 ] ) -swarmbot( name "r1853" pose [ 42.500 -14.250 0 0 ] ) -swarmbot( name "r1854" pose [ 42.500 -14.000 0 0 ] ) -swarmbot( name "r1855" pose [ 42.750 -15.000 0 0 ] ) -swarmbot( name "r1856" pose [ 42.750 -14.750 0 0 ] ) -swarmbot( name "r1857" pose [ 42.750 -14.500 0 0 ] ) -swarmbot( name "r1858" pose [ 42.750 -14.250 0 0 ] ) -swarmbot( name "r1859" pose [ 42.750 -14.000 0 0 ] ) -swarmbot( name "r1860" pose [ 43.000 -15.000 0 0 ] ) -swarmbot( name "r1861" pose [ 43.000 -14.750 0 0 ] ) -swarmbot( name "r1862" pose [ 43.000 -14.500 0 0 ] ) -swarmbot( name "r1863" pose [ 43.000 -14.250 0 0 ] ) -swarmbot( name "r1864" pose [ 43.000 -14.000 0 0 ] ) -swarmbot( name "r1865" pose [ 43.250 -15.000 0 0 ] ) -swarmbot( name "r1866" pose [ 43.250 -14.750 0 0 ] ) -swarmbot( name "r1867" pose [ 43.250 -14.500 0 0 ] ) -swarmbot( name "r1868" pose [ 43.250 -14.250 0 0 ] ) -swarmbot( name "r1869" pose [ 43.250 -14.000 0 0 ] ) -swarmbot( name "r1870" pose [ 43.500 -15.000 0 0 ] ) -swarmbot( name "r1871" pose [ 43.500 -14.750 0 0 ] ) -swarmbot( name "r1872" pose [ 43.500 -14.500 0 0 ] ) -swarmbot( name "r1873" pose [ 43.500 -14.250 0 0 ] ) -swarmbot( name "r1874" pose [ 43.500 -14.000 0 0 ] ) -swarmbot( name "r1875" pose [ 43.750 -15.000 0 0 ] ) -swarmbot( name "r1876" pose [ 43.750 -14.750 0 0 ] ) -swarmbot( name "r1877" pose [ 43.750 -14.500 0 0 ] ) -swarmbot( name "r1878" pose [ 43.750 -14.250 0 0 ] ) -swarmbot( name "r1879" pose [ 43.750 -14.000 0 0 ] ) -swarmbot( name "r1880" pose [ 44.000 -15.000 0 0 ] ) -swarmbot( name "r1881" pose [ 44.000 -14.750 0 0 ] ) -swarmbot( name "r1882" pose [ 44.000 -14.500 0 0 ] ) -swarmbot( name "r1883" pose [ 44.000 -14.250 0 0 ] ) -swarmbot( name "r1884" pose [ 44.000 -14.000 0 0 ] ) -swarmbot( name "r1885" pose [ 44.250 -15.000 0 0 ] ) -swarmbot( name "r1886" pose [ 44.250 -14.750 0 0 ] ) -swarmbot( name "r1887" pose [ 44.250 -14.500 0 0 ] ) -swarmbot( name "r1888" pose [ 44.250 -14.250 0 0 ] ) -swarmbot( name "r1889" pose [ 44.250 -14.000 0 0 ] ) -swarmbot( name "r1890" pose [ 44.500 -15.000 0 0 ] ) -swarmbot( name "r1891" pose [ 44.500 -14.750 0 0 ] ) -swarmbot( name "r1892" pose [ 44.500 -14.500 0 0 ] ) -swarmbot( name "r1893" pose [ 44.500 -14.250 0 0 ] ) -swarmbot( name "r1894" pose [ 44.500 -14.000 0 0 ] ) -swarmbot( name "r1895" pose [ 44.750 -15.000 0 0 ] ) -swarmbot( name "r1896" pose [ 44.750 -14.750 0 0 ] ) -swarmbot( name "r1897" pose [ 44.750 -14.500 0 0 ] ) -swarmbot( name "r1898" pose [ 44.750 -14.250 0 0 ] ) -swarmbot( name "r1899" pose [ 44.750 -14.000 0 0 ] ) -swarmbot( name "r1900" pose [ 45.000 -15.000 0 0 ] ) -swarmbot( name "r1901" pose [ 45.000 -14.750 0 0 ] ) -swarmbot( name "r1902" pose [ 45.000 -14.500 0 0 ] ) -swarmbot( name "r1903" pose [ 45.000 -14.250 0 0 ] ) -swarmbot( name "r1904" pose [ 45.000 -14.000 0 0 ] ) -swarmbot( name "r1905" pose [ 45.250 -15.000 0 0 ] ) -swarmbot( name "r1906" pose [ 45.250 -14.750 0 0 ] ) -swarmbot( name "r1907" pose [ 45.250 -14.500 0 0 ] ) -swarmbot( name "r1908" pose [ 45.250 -14.250 0 0 ] ) -swarmbot( name "r1909" pose [ 45.250 -14.000 0 0 ] ) -swarmbot( name "r1910" pose [ 45.500 -15.000 0 0 ] ) -swarmbot( name "r1911" pose [ 45.500 -14.750 0 0 ] ) -swarmbot( name "r1912" pose [ 45.500 -14.500 0 0 ] ) -swarmbot( name "r1913" pose [ 45.500 -14.250 0 0 ] ) -swarmbot( name "r1914" pose [ 45.500 -14.000 0 0 ] ) -swarmbot( name "r1915" pose [ 45.750 -15.000 0 0 ] ) -swarmbot( name "r1916" pose [ 45.750 -14.750 0 0 ] ) -swarmbot( name "r1917" pose [ 45.750 -14.500 0 0 ] ) -swarmbot( name "r1918" pose [ 45.750 -14.250 0 0 ] ) -swarmbot( name "r1919" pose [ 45.750 -14.000 0 0 ] ) -swarmbot( name "r1920" pose [ 46.000 -15.000 0 0 ] ) -swarmbot( name "r1921" pose [ 46.000 -14.750 0 0 ] ) -swarmbot( name "r1922" pose [ 46.000 -14.500 0 0 ] ) -swarmbot( name "r1923" pose [ 46.000 -14.250 0 0 ] ) -swarmbot( name "r1924" pose [ 46.000 -14.000 0 0 ] ) -swarmbot( name "r1925" pose [ 46.250 -15.000 0 0 ] ) -swarmbot( name "r1926" pose [ 46.250 -14.750 0 0 ] ) -swarmbot( name "r1927" pose [ 46.250 -14.500 0 0 ] ) -swarmbot( name "r1928" pose [ 46.250 -14.250 0 0 ] ) -swarmbot( name "r1929" pose [ 46.250 -14.000 0 0 ] ) -swarmbot( name "r1930" pose [ 46.500 -15.000 0 0 ] ) -swarmbot( name "r1931" pose [ 46.500 -14.750 0 0 ] ) -swarmbot( name "r1932" pose [ 46.500 -14.500 0 0 ] ) -swarmbot( name "r1933" pose [ 46.500 -14.250 0 0 ] ) -swarmbot( name "r1934" pose [ 46.500 -14.000 0 0 ] ) -swarmbot( name "r1935" pose [ 46.750 -15.000 0 0 ] ) -swarmbot( name "r1936" pose [ 46.750 -14.750 0 0 ] ) -swarmbot( name "r1937" pose [ 46.750 -14.500 0 0 ] ) -swarmbot( name "r1938" pose [ 46.750 -14.250 0 0 ] ) -swarmbot( name "r1939" pose [ 46.750 -14.000 0 0 ] ) -swarmbot( name "r1940" pose [ 47.000 -15.000 0 0 ] ) -swarmbot( name "r1941" pose [ 47.000 -14.750 0 0 ] ) -swarmbot( name "r1942" pose [ 47.000 -14.500 0 0 ] ) -swarmbot( name "r1943" pose [ 47.000 -14.250 0 0 ] ) -swarmbot( name "r1944" pose [ 47.000 -14.000 0 0 ] ) -swarmbot( name "r1945" pose [ 47.250 -15.000 0 0 ] ) -swarmbot( name "r1946" pose [ 47.250 -14.750 0 0 ] ) -swarmbot( name "r1947" pose [ 47.250 -14.500 0 0 ] ) -swarmbot( name "r1948" pose [ 47.250 -14.250 0 0 ] ) -swarmbot( name "r1949" pose [ 47.250 -14.000 0 0 ] ) -swarmbot( name "r1950" pose [ 47.500 -15.000 0 0 ] ) -swarmbot( name "r1951" pose [ 47.500 -14.750 0 0 ] ) -swarmbot( name "r1952" pose [ 47.500 -14.500 0 0 ] ) -swarmbot( name "r1953" pose [ 47.500 -14.250 0 0 ] ) -swarmbot( name "r1954" pose [ 47.500 -14.000 0 0 ] ) -swarmbot( name "r1955" pose [ 47.750 -15.000 0 0 ] ) -swarmbot( name "r1956" pose [ 47.750 -14.750 0 0 ] ) -swarmbot( name "r1957" pose [ 47.750 -14.500 0 0 ] ) -swarmbot( name "r1958" pose [ 47.750 -14.250 0 0 ] ) -swarmbot( name "r1959" pose [ 47.750 -14.000 0 0 ] ) -swarmbot( name "r1960" pose [ 48.000 -15.000 0 0 ] ) -swarmbot( name "r1961" pose [ 48.000 -14.750 0 0 ] ) -swarmbot( name "r1962" pose [ 48.000 -14.500 0 0 ] ) -swarmbot( name "r1963" pose [ 48.000 -14.250 0 0 ] ) -swarmbot( name "r1964" pose [ 48.000 -14.000 0 0 ] ) -swarmbot( name "r1965" pose [ 48.250 -15.000 0 0 ] ) -swarmbot( name "r1966" pose [ 48.250 -14.750 0 0 ] ) -swarmbot( name "r1967" pose [ 48.250 -14.500 0 0 ] ) -swarmbot( name "r1968" pose [ 48.250 -14.250 0 0 ] ) -swarmbot( name "r1969" pose [ 48.250 -14.000 0 0 ] ) -swarmbot( name "r1970" pose [ 48.500 -15.000 0 0 ] ) -swarmbot( name "r1971" pose [ 48.500 -14.750 0 0 ] ) -swarmbot( name "r1972" pose [ 48.500 -14.500 0 0 ] ) -swarmbot( name "r1973" pose [ 48.500 -14.250 0 0 ] ) -swarmbot( name "r1974" pose [ 48.500 -14.000 0 0 ] ) -swarmbot( name "r1975" pose [ 48.750 -15.000 0 0 ] ) -swarmbot( name "r1976" pose [ 48.750 -14.750 0 0 ] ) -swarmbot( name "r1977" pose [ 48.750 -14.500 0 0 ] ) -swarmbot( name "r1978" pose [ 48.750 -14.250 0 0 ] ) -swarmbot( name "r1979" pose [ 48.750 -14.000 0 0 ] ) -swarmbot( name "r1980" pose [ 49.000 -15.000 0 0 ] ) -swarmbot( name "r1981" pose [ 49.000 -14.750 0 0 ] ) -swarmbot( name "r1982" pose [ 49.000 -14.500 0 0 ] ) -swarmbot( name "r1983" pose [ 49.000 -14.250 0 0 ] ) -swarmbot( name "r1984" pose [ 49.000 -14.000 0 0 ] ) -swarmbot( name "r1985" pose [ 49.250 -15.000 0 0 ] ) -swarmbot( name "r1986" pose [ 49.250 -14.750 0 0 ] ) -swarmbot( name "r1987" pose [ 49.250 -14.500 0 0 ] ) -swarmbot( name "r1988" pose [ 49.250 -14.250 0 0 ] ) -swarmbot( name "r1989" pose [ 49.250 -14.000 0 0 ] ) -swarmbot( name "r1990" pose [ 49.500 -15.000 0 0 ] ) -swarmbot( name "r1991" pose [ 49.500 -14.750 0 0 ] ) -swarmbot( name "r1992" pose [ 49.500 -14.500 0 0 ] ) -swarmbot( name "r1993" pose [ 49.500 -14.250 0 0 ] ) -swarmbot( name "r1994" pose [ 49.500 -14.000 0 0 ] ) -swarmbot( name "r1995" pose [ 49.750 -15.000 0 0 ] ) -swarmbot( name "r1996" pose [ 49.750 -14.750 0 0 ] ) -swarmbot( name "r1997" pose [ 49.750 -14.500 0 0 ] ) -swarmbot( name "r1998" pose [ 49.750 -14.250 0 0 ] ) -swarmbot( name "r1999" pose [ 49.750 -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 ] ) +#swarm... [truncated message content] |
From: <rt...@us...> - 2008-11-15 03:11:50
|
Revision: 7152 http://playerstage.svn.sourceforge.net/playerstage/?rev=7152&view=rev Author: rtv Date: 2008-11-15 03:11:44 +0000 (Sat, 15 Nov 2008) Log Message: ----------- major changes for 3.1 Modified Paths: -------------- code/stage/trunk/CMakeLists.txt 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 Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-11-15 03:11:20 UTC (rev 7151) +++ code/stage/trunk/CMakeLists.txt 2008-11-15 03:11:44 UTC (rev 7152) @@ -1,8 +1,8 @@ PROJECT(Stage) SET( V_MAJOR 3 ) -SET( V_MINOR 0 ) -SET( V_BUGFIX 1 ) +SET( V_MINOR 1 ) +SET( V_BUGFIX 0 ) SET( VERSION ${V_MAJOR}.${V_MINOR}.${V_BUGFIX} ) SET( APIVERSION ${V_MAJOR}.${V_MINOR} ) @@ -57,7 +57,7 @@ MESSAGE( STATUS "Checking for required libraries..." ) SET( INDENT " * " ) -pkg_search_module( GLIB REQUIRED glib-2.0 ) +pkg_search_module( GLIB REQUIRED gthread-2.0 ) IF( GLIB_FOUND ) MESSAGE( ${INDENT} "Glib version ${GLIB_VERSION} detected at ${GLIB_PREFIX}" ) MESSAGE( " GLIB_CFLAGS = ${GLIB_CFLAGS}" ) Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-11-15 03:11:20 UTC (rev 7151) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-15 03:11:44 UTC (rev 7152) @@ -45,9 +45,10 @@ int LaserUpdate( StgModel* mod, robot_t* robot ); int PositionUpdate( StgModel* mod, robot_t* robot ); + // Stage calls this when the model starts up extern "C" int Init( StgModel* mod ) -{ +{ robot_t* robot = new robot_t; robot->work_get = 0; robot->work_put = 0; @@ -69,7 +70,7 @@ robot->sink = mod->GetWorld()->GetModel( "sink" ); assert(robot->sink); - + return 0; //ok } @@ -93,38 +94,61 @@ for (uint32_t i = 0; i < sample_count; i++) { + + if( verbose ) printf( "%.3f ", scan[i].range ); + if( (i > (sample_count/4)) - && (i < (sample_count - (sample_count/4))) - && scan[i].range < minfrontdistance) - obstruction = true; - + && (i < (sample_count - (sample_count/4))) + && scan[i].range < minfrontdistance) + { + if( verbose ) puts( " obstruction!" ); + obstruction = true; + } + if( scan[i].range < stopdist ) - stop = true; + { + if( verbose ) puts( " stopping!" ); + stop = true; + } if( i > sample_count/2 ) - minleft = MIN( minleft, scan[i].range ); + minleft = MIN( minleft, scan[i].range ); else - minright = MIN( minright, scan[i].range ); + minright = MIN( minright, scan[i].range ); } + if( verbose ) + { + puts( "" ); + printf( "minleft %.3f \n", minleft ); + printf( "minright %.3f\n ", minright ); + } + if( obstruction || stop || (robot->avoidcount>0) ) { - if( verbose ) puts( "Avoid" ); + if( verbose ) printf( "Avoid %d\n", robot->avoidcount ); + robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed ); /* once we start avoiding, select a turn direction and stick with it for a few iterations */ if( robot->avoidcount < 1 ) { - if( verbose ) puts( "Avoid START" ); + if( verbose ) puts( "Avoid START" ); robot->avoidcount = random() % avoidduration + avoidduration; - - if( minleft < minright ) - robot->pos->SetTurnSpeed( -avoidturn ); - else - robot->pos->SetTurnSpeed( +avoidturn ); + + if( minleft < minright ) + { + robot->pos->SetTurnSpeed( -avoidturn ); + if( verbose ) printf( "turning right %.2f\n", -avoidturn ); + } + else + { + robot->pos->SetTurnSpeed( +avoidturn ); + if( verbose ) printf( "turning left %2f\n", +avoidturn ); + } } - + robot->avoidcount--; } else @@ -140,18 +164,19 @@ int y = (pose.y + 8) / 4; double a_goal = - dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] ); + dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] ); double a_error = normalize( a_goal - pose.a ); - + robot->pos->SetTurnSpeed( a_error ); } - + + return 0; } int PositionUpdate( StgModel* mod, robot_t* robot ) -{ +{ stg_pose_t pose = robot->pos->GetPose(); //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", @@ -159,28 +184,40 @@ //pose.z += 0.0001; //robot->pos->SetPose( pose ); - + if( robot->pos->GetFlagCount() < payload && hypot( -7-pose.x, -7-pose.y ) < 2.0 ) { if( ++robot->work_get > workduration ) - { - robot->pos->PushFlag( robot->source->PopFlag() ); - robot->work_get = 0; - } + { + // protect source from concurrent access + robot->source->Lock(); + + // transfer a chunk from source to robot + robot->pos->PushFlag( robot->source->PopFlag() ); + robot->source->Unlock(); + + robot->work_get = 0; + } } if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) { if( ++robot->work_put > workduration ) - { - //puts( "dropping" ); - // transfer a chunk between robot and goal - robot->sink->PushFlag( robot->pos->PopFlag() ); - robot->work_put = 0; - } + { + // protect sink from concurrent access + robot->sink->Lock(); + + //puts( "dropping" ); + // transfer a chunk between robot and goal + robot->sink->PushFlag( robot->pos->PopFlag() ); + robot->sink->Unlock(); + + robot->work_put = 0; + } } + return 0; // run again } Modified: code/stage/trunk/examples/ctrl/sink.cc =================================================================== --- code/stage/trunk/examples/ctrl/sink.cc 2008-11-15 03:11:20 UTC (rev 7151) +++ code/stage/trunk/examples/ctrl/sink.cc 2008-11-15 03:11:44 UTC (rev 7152) @@ -20,9 +20,13 @@ // inspect the laser data and decide what to do int Update( StgModel* 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 2008-11-15 03:11:20 UTC (rev 7151) +++ code/stage/trunk/examples/ctrl/source.cc 2008-11-15 03:11:44 UTC (rev 7152) @@ -10,21 +10,24 @@ // Stage calls this when the model starts up extern "C" int Init( StgModel* mod ) { - for( int i=0; i<5; i++ ) mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0 ), flagsz ) ); mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); - + return 0; //ok } // inspect the laser data and decide what to do int Update( StgModel* mod, void* dummy ) { - + // protect access to this model from other controllers + mod->Lock(); + if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 ) mod->PushFlag( new StgFlag( 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 2008-11-15 03:11:20 UTC (rev 7151) +++ code/stage/trunk/examples/ctrl/wander.cc 2008-11-15 03:11:44 UTC (rev 7152) @@ -3,9 +3,11 @@ const double cruisespeed = 0.4; const double avoidspeed = 0.05; -const double avoidturn = 1.0; -const double minfrontdistance = 0.8; +const double avoidturn = 0.5; +const double minfrontdistance = 0.6; const bool verbose = false; +const double stopdist = 0.3; +const int avoidduration = 10; typedef struct { @@ -33,6 +35,7 @@ return 0; //ok } + // inspect the laser data and decide what to do int LaserUpdate( StgModel* mod, robot_t* robot ) { @@ -41,44 +44,71 @@ stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); assert(scan); - double newturnrate=0.0, newspeed=0.0; bool obstruction = false; - + bool stop = false; + // find the closest distance to the left and right and check if // there's anything in front double minleft = 1e6; double minright = 1e6; - + for (uint32_t i = 0; i < sample_count; i++) { - if( scan[i].range < minfrontdistance) - obstruction = true; + + if( verbose ) printf( "%.3f ", scan[i].range ); + + if( (i > (sample_count/3)) + && (i < (sample_count - (sample_count/3))) + && scan[i].range < minfrontdistance) + { + if( verbose ) puts( " obstruction!" ); + obstruction = true; + } + + if( scan[i].range < stopdist ) + { + if( verbose ) puts( " stopping!" ); + stop = true; + } if( i > sample_count/2 ) - minleft = MIN( minleft, scan[i].range ); + minleft = MIN( minleft, scan[i].range ); else - minright = MIN( minright, scan[i].range ); + minright = MIN( minright, scan[i].range ); } - if( obstruction || robot->avoidcount ) + if( verbose ) + { + puts( "" ); + printf( "minleft %.3f \n", minleft ); + printf( "minright %.3f\n ", minright ); + } + + if( obstruction || stop || (robot->avoidcount>0) ) { - if( verbose ) puts( "Avoid" ); - - robot->pos->SetXSpeed( avoidspeed ); + if( verbose ) printf( "Avoid %d\n", robot->avoidcount ); + + robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed ); /* once we start avoiding, select a turn direction and stick with it for a few iterations */ - if( robot->avoidcount == 0 ) + if( robot->avoidcount < 1 ) { - if( verbose ) puts( "Avoid START" ); - robot->avoidcount = 5; - - if( minleft < minright ) - robot->pos->SetTurnSpeed( -avoidturn ); - else - robot->pos->SetTurnSpeed( +avoidturn ); + if( verbose ) puts( "Avoid START" ); + robot->avoidcount = random() % avoidduration + avoidduration; + + if( minleft < minright ) + { + robot->pos->SetTurnSpeed( -avoidturn ); + if( verbose ) printf( "turning right %.2f\n", -avoidturn ); + } + else + { + robot->pos->SetTurnSpeed( +avoidturn ); + if( verbose ) printf( "turning left %2f\n", +avoidturn ); + } } - + robot->avoidcount--; } else @@ -87,23 +117,9 @@ robot->avoidcount = 0; robot->pos->SetXSpeed( cruisespeed ); - - /* update turnrate every few updates */ - if( robot->randcount == 0 ) - { - if( verbose )puts( "Random turn" ); - - /* make random int tween -30 and 30 */ - //newturnrate = dtor( rand() % 61 - 30 ); - - robot->randcount = 20; - - robot->pos->SetTurnSpeed( dtor( rand() % 11 - 5 ) ); - } - - robot->randcount--; + robot->pos->SetTurnSpeed( 0 ); } - + return 0; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-17 23:06:53
|
Revision: 7156 http://playerstage.svn.sourceforge.net/playerstage/?rev=7156&view=rev Author: rtv Date: 2008-11-17 23:06:46 +0000 (Mon, 17 Nov 2008) Log Message: ----------- reduced CPU use on idle, and eliminated superfluous screen redraws - some models may need to be tweaked to force screen updates in some unusual situations Modified Paths: -------------- code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/benchmark/cave.world code/stage/trunk/worlds/benchmark/hospital.world code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/libstage/blockgroup.cc 2008-11-17 23:06:46 UTC (rev 7156) @@ -176,9 +176,7 @@ else blk->DrawSolid(); } - - // LISTMETHOD( blocks, StgBlock*, DrawSolid ); - + glDepthMask(GL_TRUE); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/libstage/canvas.cc 2008-11-17 23:06:46 UTC (rev 7156) @@ -25,20 +25,23 @@ static const int checkImageHeight = 2; static GLubyte checkImage[checkImageHeight][checkImageWidth][4]; static GLuint texName; +static bool blur = true; + void StgCanvas::TimerCallback( StgCanvas* c ) { - c->redraw(); + if( c->world->dirty ) + { + //puts( "timer redraw" ); + c->redraw(); + c->world->dirty = false; + } Fl::repeat_timeout(((double)c->interval/1000), (Fl_Timeout_Handler)StgCanvas::TimerCallback, c); } - - - - StgCanvas::StgCanvas( StgWorldGui* world, int x, int y, int w, int h) : Fl_Gl_Window(x,y,w,h), // initialize Option objects @@ -76,13 +79,17 @@ startx = starty = -1; interval = 50; //msec between redraws + + // enable accumulation buffer + mode( mode() | FL_ACCUM ); + assert( can_do( FL_ACCUM ) ); graphics = true; - // // start the timer that causes regular redraws - Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, - this); +// // // start the timer that causes regular redraws +// Fl::add_timeout( ((double)interval/1000), +// (Fl_Timeout_Handler)StgCanvas::TimerCallback, +// this); GLenum status; @@ -183,7 +190,9 @@ { last_selection = mod; selected_models = g_list_prepend( selected_models, mod ); + // mod->Disable(); + redraw(); } } @@ -196,6 +205,7 @@ selected_models = g_list_remove_link( selected_models, link ); // mod->Enable(); + redraw(); } } } @@ -415,6 +425,19 @@ { case 'p': // pause world->TogglePause(); + + if( ! world->paused ) + { + // // start the timer that causes regular redraws + Fl::add_timeout( ((double)interval/1000), + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + this); + } + else + { // remove the timeout + Fl::remove_timeout( (Fl_Timeout_Handler)StgCanvas::TimerCallback ); + } + break; case ' ': // space bar @@ -503,16 +526,14 @@ glColor3f( 1.0, 1.0, 1.0 ); glBegin(GL_QUADS); - - glTexCoord2f( bounds.x.min/2.0, bounds.y.min/2.0 ); - glVertex2f( bounds.x.min, bounds.y.min ); - glTexCoord2f( bounds.x.max/2.0, bounds.y.min/2.0); - glVertex2f( bounds.x.max, bounds.y.min ); - glTexCoord2f( bounds.x.max/2.0, bounds.y.max/2.0 ); - glVertex2f( bounds.x.max, bounds.y.max ); - glTexCoord2f( bounds.x.min/2.0, bounds.y.max/2.0 ); - glVertex2f( bounds.x.min, bounds.y.max ); - + glTexCoord2f( bounds.x.min/2.0, bounds.y.min/2.0 ); + glVertex2f( bounds.x.min, bounds.y.min ); + glTexCoord2f( bounds.x.max/2.0, bounds.y.min/2.0); + glVertex2f( bounds.x.max, bounds.y.min ); + glTexCoord2f( bounds.x.max/2.0, bounds.y.max/2.0 ); + glVertex2f( bounds.x.max, bounds.y.max ); + glTexCoord2f( bounds.x.min/2.0, bounds.y.max/2.0 ); + glVertex2f( bounds.x.min, bounds.y.max ); glEnd(); glDisable(GL_TEXTURE_2D); @@ -628,10 +649,6 @@ glPopMatrix(); } - if( showGrid ) - DrawGlobalGrid(); - else - DrawFloor(); if( showFootprints ) { @@ -642,10 +659,95 @@ } glEnable( GL_DEPTH_TEST ); } + + if( showGrid ) + DrawGlobalGrid(); + else + DrawFloor(); if( showBlocks ) - DrawBlocks(); - + DrawBlocks(); + + +// MOTION BLUR +// if( showBlocks ) +// { +// DrawBlocks(); + +// static float count = 0; + +// if( ! blur ) +// { +// blur = true; +// glClear( GL_ACCUM_BUFFER_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); +// glAccum( GL_LOAD, 1.0 ); +// } +// else +// { +// glAccum( GL_MULT, 0.9 ); +// glAccum( GL_ACCUM, 0.1 ); + +// glAccum( GL_RETURN, 1.1 ); + +// DrawBlocks(); // outline at current location +// } +// } + +// GRAY TRAILS +// if( showBlocks ) +// { + +// static float count = 0; + +// if( ! blur ) +// { +// blur = true; +// glClear( GL_ACCUM_BUFFER_BIT ); +// DrawBlocks(); +// glAccum( GL_LOAD, 1.0 ); +// } +// else +// { +// glAccum( GL_MULT, 0.9 ); + +// DrawBlocks(); + +// glAccum( GL_ACCUM, 0.1); + +// glAccum( GL_RETURN, 1.0 ); + +// DrawBlocks(); +// } +// } + +// PRETTY BLACK +// if( showBlocks ) +// { + +// static float count = 0; + +// if( ! blur ) +// { +// blur = true; +// glClear( GL_ACCUM_BUFFER_BIT ); +// DrawBlocks(); +// glAccum( GL_LOAD, 1.0 ); +// } +// else +// { +// glAccum( GL_MULT, 0.9 ); +// glAccum( GL_RETURN, 1.0 ); + +// DrawBlocks(); + +// glAccum( GL_ACCUM, 0.1); + +// glAccum( GL_RETURN, 1.0 ); + +// DrawBlocks(); +// } +// } + if( showTrailRise ) { for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { @@ -908,6 +1010,12 @@ showScreenshots.Load( wf, sec ); pCamOn.Load( wf, sec ); + if( ! world->paused ) + // // start the timer that causes regular redraws + Fl::add_timeout( ((double)interval/1000), + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + this); + invalidate(); // we probably changed something } @@ -937,6 +1045,9 @@ void StgCanvas::draw() { +// static unsigned long calls=0; +// printf( "Draw calls %lu\n", ++calls ); + static bool loaded_texture = false; //Enable the following to debug camera model @@ -964,6 +1075,8 @@ glEnableClientState( GL_VERTEX_ARRAY ); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + blur = false; + //TODO find a better home for loading textures if( loaded_texture == false ) { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/libstage/model.cc 2008-11-17 23:06:46 UTC (rev 7156) @@ -159,7 +159,6 @@ parent(parent), type(type), id( StgModel::count++ ), - gpose_dirty(true), trail( g_array_new( false, false, sizeof(stg_trail_item_t) )), blocks_dl(0), data_fresh(false), @@ -553,28 +552,20 @@ { //printf( "model %s global pose ", token ); - // if( this->gpose_dirty ) - { - stg_pose_t parent_pose; - - // find my parent's pose - if( this->parent ) - { - parent_pose = parent->GetGlobalPose(); - stg_pose_sum( &global_pose, &parent_pose, &pose ); - - // we are on top of our parent - global_pose.z += parent->geom.size.z; - } - else - memcpy( &global_pose, &pose, sizeof(stg_pose_t)); + stg_pose_t parent_pose; + + // find my parent's pose + if( this->parent ) + { + parent_pose = parent->GetGlobalPose(); + stg_pose_sum( &global_pose, &parent_pose, &pose ); - this->gpose_dirty = false; - //printf( " WORK " ); - } - //else - //printf( " CACHED " ); - + // we are on top of our parent + global_pose.z += parent->geom.size.z; + } + else + memcpy( &global_pose, &pose, sizeof(stg_pose_t)); + // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]", // global_pose.x, // global_pose.y, @@ -1301,15 +1292,17 @@ if( parent ) parent->NeedRedraw(); + else + world->NeedRedraw(); } -void StgModel::GPoseDirtyTree( void ) -{ - this->gpose_dirty = true; // our global pose may have changed +// void StgModel::GPoseDirtyTree( void ) +// { +// this->gpose_dirty = true; // our global pose may have changed - for( GList* it = this->children; it; it=it->next ) - ((StgModel*)it->data)->GPoseDirtyTree(); -} +// for( GList* it = this->children; it; it=it->next ) +// ((StgModel*)it->data)->GPoseDirtyTree(); +// } void StgModel::SetPose( stg_pose_t pose ) { @@ -1329,10 +1322,11 @@ this->pose.a = normalize(this->pose.a); this->NeedRedraw(); - this->GPoseDirtyTree(); // global pose may have changed this->map_caches_are_invalid = true; MapWithChildren(); + + world->dirty = true; } // register a model change even if the pose didn't actually change @@ -1365,8 +1359,6 @@ // geom->pose.x, geom->pose.y, geom->pose.a, // geom->size.x, geom->size.y ); - this->gpose_dirty = true; - UnMapWithChildren(); this->geom = geom; @@ -1553,8 +1545,11 @@ if( hitmod ) pose = startpose; // move failed - put me back where I started else - CommitTestedPose(); // shift anyrecursively commit to blocks to the new pose - + { + CommitTestedPose(); // shift anyrecursively commit to blocks to the new pose + world->dirty = true; // need redraw + } + return hitmod; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/libstage/stage.hh 2008-11-17 23:06:46 UTC (rev 7156) @@ -883,6 +883,9 @@ /** StgWorld::quit is set true when this simulation time is reached */ stg_usec_t quit_time; + // hint that the world needs to be redrawn if a GUI is attached + void NeedRedraw(){ dirty = true; }; + 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/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/libstage/worldgui.cc 2008-11-17 23:06:46 UTC (rev 7156) @@ -298,20 +298,35 @@ Fl::check(); timenow = RealTimeNow(); - interval = timenow - real_time_of_last_update; // guaranteed to be >= 0 - double sleeptime = (double)interval_real - (double)interval; - - if( sleeptime > 0 ) - usleep( (stg_usec_t)MIN(sleeptime,100000) ); // check the GUI at 10Hz min - + // if we're attempting to match some real time interval + //if( interval_real > 0 ) + { + interval = timenow - real_time_of_last_update; // guaranteed to be >= 0 + + double sleeptime = (double)interval_real - (double)interval; + + if( paused ) sleeptime = 20000; // spare the CPU if we're paused + + // printf( "real %.2f interval %.2f sleeptime %.2f\n", + // (double)interval_real, + // (double)interval, + // sleeptime ); + + if( sleeptime > 0 ) + usleep( (stg_usec_t)MIN(sleeptime,20000) ); // check the GUI at 10Hz min + } } while( interval < interval_real ); + // if( paused ) // gentle on the CPU when paused + //usleep( 10000 ); interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_of_last_update; real_time_of_last_update = timenow; + //puts( "FINSHED UPDATE" ); + return val; } Modified: code/stage/trunk/worlds/benchmark/cave.world =================================================================== --- code/stage/trunk/worlds/benchmark/cave.world 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/worlds/benchmark/cave.world 2008-11-17 23:06:46 UTC (rev 7156) @@ -11,7 +11,7 @@ interval_real 0 # real-time interval between simulation updates in milliseconds paused 1 -threadpool 2 +threadpool 0 # configure the GUI window window Modified: code/stage/trunk/worlds/benchmark/hospital.world =================================================================== --- code/stage/trunk/worlds/benchmark/hospital.world 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/worlds/benchmark/hospital.world 2008-11-17 23:06:46 UTC (rev 7156) @@ -23,11 +23,8 @@ rotate [ 38.000 -36.500 ] scale 12.611 -# size [ 640 480 ] -# center [929.990 -377.040] -# rotate [ 0.000 0.000 ] -# scale 20.806 show_data 0 + interval 200 ) # load an environment bitmap Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/worlds/fasr.world 2008-11-17 23:06:46 UTC (rev 7156) @@ -8,13 +8,14 @@ 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 # threads don't help here as the per-robot update is very fast threadpool 0 + # configure the GUI window window ( @@ -65,27 +66,27 @@ ctrl "fasr" ) -autorob( pose [5.133 4.841 0 -179.934] ) -autorob( pose [6.381 5.316 0 -85.327] ) -autorob( pose [5.972 4.982 0 -41.506] ) -autorob( pose [7.627 6.296 0 -120.309] ) -autorob( pose [5.509 6.440 0 -104.877] ) -autorob( pose [7.024 6.433 0 125.724] ) -autorob( pose [5.688 4.291 0 -12.027] ) -autorob( pose [4.887 6.172 0 -144.634] ) -autorob( pose [7.135 5.076 0 -162.054] ) -autorob( pose [7.409 5.691 0 146.028] ) +autorob( pose [5.099 4.804 0 -73.937] ) +autorob( pose [6.471 5.304 0 14.941] ) +autorob( pose [5.937 4.858 0 -147.503] ) +autorob( pose [7.574 6.269 0 -111.715] ) +autorob( pose [5.664 5.938 0 -51.799] ) +autorob( pose [7.016 6.428 0 -128.279] ) +autorob( pose [5.911 4.040 0 -97.047] ) +autorob( pose [4.909 6.097 0 -44.366] ) +autorob( pose [6.898 4.775 0 -117.576] ) +autorob( pose [7.012 5.706 0 129.497] ) -autorob( pose [6.752 6.937 0 -103.060] ) -autorob( pose [6.546 4.535 0 -153.940] ) -autorob( pose [5.642 7.205 0 97.307] ) -autorob( pose [4.386 5.877 0 -35.838] ) -autorob( pose [4.892 4.191 0 63.840] ) -autorob( pose [4.516 5.050 0 -76.859] ) -autorob( pose [5.354 5.353 0 -63.787] ) -autorob( pose [7.566 6.939 0 57.242] ) -autorob( pose [7.604 4.795 0 -147.661] ) -autorob( pose [6.284 3.807 0 -145.026] ) +autorob( pose [6.616 6.893 0 170.743] ) +autorob( pose [6.451 4.189 0 -61.453] ) +autorob( pose [5.098 6.788 0 -61.295] ) +autorob( pose [4.374 5.163 0 -90.417] ) +autorob( pose [4.999 4.230 0 -42.157] ) +autorob( pose [4.331 4.217 0 -95.000] ) +autorob( pose [5.440 5.317 0 -26.545] ) +autorob( pose [7.518 6.973 0 163.239] ) +autorob( pose [7.559 4.764 0 -139.066] ) +autorob( pose [4.839 3.595 0 -179.567] ) #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [6.203 6.963 0 2.937] ) Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2008-11-17 21:08:21 UTC (rev 7155) +++ code/stage/trunk/worlds/simple.world 2008-11-17 23:06:46 UTC (rev 7156) @@ -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 1 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-19 08:22:39
|
Revision: 7159 http://playerstage.svn.sourceforge.net/playerstage/?rev=7159&view=rev Author: rtv Date: 2008-11-19 08:22:32 +0000 (Wed, 19 Nov 2008) Log Message: ----------- geometry fixes & graphics tweaks 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/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/pioneer.inc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -32,6 +32,7 @@ { StgModelPosition* pos; StgModelLaser* laser; + StgModelRanger* ranger; StgModelBlobfinder* blobfinder; StgModel *source, *sink; @@ -59,6 +60,11 @@ assert( robot->laser ); robot->laser->Subscribe(); + robot->ranger = (StgModelRanger*)mod->GetModel( "ranger:0" ); + assert( robot->ranger ); + robot->ranger->Subscribe(); + + robot->avoidcount = 0; robot->randcount = 0; Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/block.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -175,6 +175,9 @@ { stg_pose_t gpose = mod->GetGlobalPose(); + // add local offset + gpose = pose_sum( gpose, mod->geom.pose ); + stg_point3_t scale; scale.x = mod->geom.size.x / mod->blockgroup.size.x; scale.y = mod->geom.size.y / mod->blockgroup.size.y; Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/blockgroup.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -190,8 +190,6 @@ if( displaylist == 0 ) BuildDisplayList( mod ); - //gl_pose_shift( &geom.pose ); - glCallList( displaylist ); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/canvas.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -30,6 +30,7 @@ static GLuint texName; static bool blur = true; +static bool init_done = false; void StgCanvas::TimerCallback( StgCanvas* c ) { @@ -45,11 +46,14 @@ c); } -StgCanvas::StgCanvas( StgWorldGui* world, int x, int y, int w, int h) : - Fl_Gl_Window(x,y,w,h), +StgCanvas::StgCanvas( StgWorldGui* world, + int x, int y, + int width, int height) : + Fl_Gl_Window( x, y, width, height ), // initialize Option objects showBlinken( "Blinkenlights", "show_blinkenlights", "", true ), showBlocks( "Blocks", "show_blocks", "b", true ), + showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false ), showClock( "Clock", "show_clock", "c", true ), showData( "Data", "show_data", "d", false ), showFlags( "Flags", "show_flags", "l", true ), @@ -63,33 +67,101 @@ showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false ), showTrails( "Trails/Fast", "show_trailfast", "^f", false ), showTree( "Debug/Tree", "show_tree", "^t", false ), + showBlur( "Trails/Blur", "show_trailblur", "^d", false ), pCamOn( "Perspective camera", "pcam_on", "r", false ), - visualizeAll( "Visualize All", "vis_all", "^v", true ) + visualizeAll( "Visualize All", "vis_all", "^v", true ), + // and the rest + world( world ), + selected_models( NULL ), + last_selection( NULL ), + wf( NULL ), + startx( -1 ), + starty( -1 ), + interval( 50 ), //msec between redraws + graphics( true ) { end(); - + //show(); // must do this so that the GL context is created before configuring GL // but that line causes a segfault in Linux/X11! TODO: test in OS X - - this->world = world; - selected_models = NULL; - last_selection = NULL; - wf = NULL; - + perspective_camera.setPose( 0.0, -4.0, 3.0 ); current_camera = &camera; setDirtyBuffer(); - startx = starty = -1; - interval = 50; //msec between redraws - // enable accumulation buffer mode( mode() | FL_ACCUM ); assert( can_do( FL_ACCUM ) ); +} - graphics = true; +void StgCanvas::InitGl() +{ + valid(1); + FixViewport(w(), h()); + + // set gl state that won't change every redraw + glClearColor ( 0.7, 0.7, 0.8, 1.0); + glDisable( GL_LIGHTING ); + glEnable( GL_DEPTH_TEST ); + glDepthFunc( GL_LESS ); + glCullFace( GL_BACK ); + glEnable (GL_CULL_FACE); + glEnable( GL_BLEND ); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); + glEnable( GL_LINE_SMOOTH ); + glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST ); + glDepthMask( GL_TRUE ); + glEnable( GL_TEXTURE_2D ); + glEnableClientState( GL_VERTEX_ARRAY ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + // install a font + gl_font( FL_HELVETICA, 12 ); + + blur = false; + + // load textures + std::string fullpath = FileManager::findFile( "assets/stall.png" ); + if ( fullpath == "" ) + { + PRINT_DEBUG( "Unable to load texture.\n" ); + } + + GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); + TextureManager::getInstance()._stall_texture_id = stall_id; + + //TODO merge this code into the textureManager? + int i, j; + for (i = 0; i < checkImageHeight; i++) + for (j = 0; j < checkImageWidth; j++) + { + int even = (i+j)%2; + checkImage[i][j][0] = (GLubyte) 255 - 10*even; + checkImage[i][j][1] = (GLubyte) 255 - 10*even; + checkImage[i][j][2] = (GLubyte) 255;// - 5*even; + checkImage[i][j][3] = 255; + } + + + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glGenTextures(1, &texName); + glBindTexture(GL_TEXTURE_2D, texName); + glEnable(GL_TEXTURE_2D); + + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight, + 0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage); + + glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); + + init_done = true; } + StgCanvas::~StgCanvas() { // nothing to do @@ -563,6 +635,20 @@ LISTMETHOD( models_sorted, StgModel*, DrawBlocksTree ); } +void StgCanvas::DrawBoundingBoxes() +{ + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glLineWidth( 2.0 ); + glPointSize( 5.0 ); + glDisable (GL_CULL_FACE); + + world->DrawBoundingBoxTree(); + + glEnable (GL_CULL_FACE); + glLineWidth( 1.0 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); +} + inline void StgCanvas::resetCamera() { float max_x = 0, max_y = 0, min_x = 0, min_y = 0; @@ -673,29 +759,32 @@ if( showBlocks ) DrawBlocks(); + if( showBBoxes ) + DrawBoundingBoxes(); + // MOTION BLUR -// if( showBlocks ) -// { -// DrawBlocks(); + if( showBlur ) + { + DrawBlocks(); -// static float count = 0; + //static float count = 0; -// if( ! blur ) -// { -// blur = true; -// glClear( GL_ACCUM_BUFFER_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); -// glAccum( GL_LOAD, 1.0 ); -// } -// else -// { -// glAccum( GL_MULT, 0.9 ); -// glAccum( GL_ACCUM, 0.1 ); + if( ! blur ) + { + blur = true; + glClear( GL_ACCUM_BUFFER_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glAccum( GL_LOAD, 1.0 ); + } + else + { + glAccum( GL_MULT, 0.9 ); + glAccum( GL_ACCUM, 0.1 ); -// glAccum( GL_RETURN, 1.1 ); + glAccum( GL_RETURN, 1.1 ); -// DrawBlocks(); // outline at current location -// } -// } + DrawBlocks(); // outline at current location + } + } // GRAY TRAILS // if( showBlocks ) @@ -981,6 +1070,8 @@ //showTrailArrows.createMenuItem( menu, path ); // broken showTrails.createMenuItem( menu, path ); // showTrailRise.createMenuItem( menu, path ); // broken + showBBoxes.createMenuItem( menu, path ); + showBlur.createMenuItem( menu, path ); showTree.createMenuItem( menu, path ); showScreenshots.createMenuItem( menu, path ); } @@ -997,6 +1088,8 @@ showData.Load( wf, sec ); showFlags.Load( wf, sec ); showBlocks.Load( wf, sec ); + showBBoxes.Load( wf, sec ); + showBlur.Load( wf, sec ); showClock.Load( wf, sec ); showFollow.Load( wf, sec ); showFootprints.Load( wf, sec ); @@ -1027,6 +1120,8 @@ showData.Save( wf, sec ); showBlocks.Save( wf, sec ); + showBBoxes.Save( wf, sec ); + showBlur.Save( wf, sec ); showClock.Save( wf, sec ); showFlags.Save( wf, sec ); showFollow.Save( wf, sec ); @@ -1047,7 +1142,6 @@ // static unsigned long calls=0; // printf( "Draw calls %lu\n", ++calls ); - static bool loaded_texture = false; //Enable the following to debug camera model // if( loaded_texture == true && pCamOn == true ) @@ -1055,75 +1149,9 @@ if (!valid() ) { - valid(1); - FixViewport(w(), h()); + if( ! init_done ) + InitGl(); - // set gl state that won't change every redraw - glClearColor ( 0.7, 0.7, 0.8, 1.0); - glDisable( GL_LIGHTING ); - glEnable( GL_DEPTH_TEST ); - glDepthFunc( GL_LESS ); - glCullFace( GL_BACK ); - glEnable (GL_CULL_FACE); - glEnable( GL_BLEND ); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA ); - glEnable( GL_LINE_SMOOTH ); - glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST ); - glDepthMask( GL_TRUE ); - glEnable( GL_TEXTURE_2D ); - glEnableClientState( GL_VERTEX_ARRAY ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - - blur = false; - - //TODO find a better home for loading textures - if( loaded_texture == false ) - { - std::string fullpath = FileManager::findFile( "assets/stall.png" ); - if ( fullpath == "" ) { - PRINT_DEBUG( "Unable to load texture.\n" ); - } - - GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); - TextureManager::getInstance()._stall_texture_id = stall_id; - - //create floor texture - { - //TODO merge this code into the textureManager - int i, j; - for (i = 0; i < checkImageHeight; i++) - for (j = 0; j < checkImageWidth; j++) - { - int even = (i+j)%2; - checkImage[i][j][0] = (GLubyte) 255 - 10*even; - checkImage[i][j][1] = (GLubyte) 255 - 10*even; - checkImage[i][j][2] = (GLubyte) 255;// - 5*even; - checkImage[i][j][3] = 255; - } - - - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); - glGenTextures(1, &texName); - glBindTexture(GL_TEXTURE_2D, texName); - glEnable(GL_TEXTURE_2D); - - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight, - 0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage); - - glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); - } - - loaded_texture = true; - } - - // install a font - gl_font( FL_HELVETICA, 12 ); - if( pCamOn == true ) { perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/model.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -923,31 +923,56 @@ } void StgModel::DrawBlocks( ) +{ + gl_pose_shift( &geom.pose ); + blockgroup.CallDisplayList( this ); +} + +void StgModel::DrawBoundingBoxTree() { - // testing - draw bounding box - // PushColor( color ); + PushLocalCoords(); + LISTMETHOD( children, StgModel*, DrawBoundingBoxTree ); + DrawBoundingBox(); + PopCoords(); +} + +void StgModel::DrawBoundingBox() +{ + gl_pose_shift( &geom.pose ); + + PushColor( color ); - // // bottom - // glBegin( GL_LINE_LOOP ); - // glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); - // glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 ); - // glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); - // glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 ); - // glEnd(); + glBegin( GL_QUAD_STRIP ); - // // top - // glBegin( GL_LINE_LOOP ); - // glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); - // glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); - // glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); - // glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); - // glEnd(); - - // PopColor(); - - blockgroup.CallDisplayList( this ); - - //printf( "calling list for %s\n", token ); + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glEnd(); + + glBegin( GL_LINES ); + glVertex2f( -0.02, 0 ); + glVertex2f( +0.02, 0 ); + + glVertex2f( 0, -0.02 ); + glVertex2f( 0, +0.02 ); + glEnd(); + + PopColor(); } // move into this model's local coordinate frame @@ -959,14 +984,6 @@ glTranslatef( 0,0, parent->geom.size.z ); gl_pose_shift( &pose ); - - // useful debug - draw a point at the local origin - // PushColor( color ); -// glPointSize( 5.0 ); -// glBegin( GL_POINTS ); -// glVertex2i( 0, 0 ); -// glEnd(); -// PopColor(); } void StgModel::PopCoords() @@ -984,8 +1001,8 @@ void StgModel::DrawStatus( Camera* cam ) { - if( say_string ) - { + if( say_string ) + { float yaw, pitch; pitch = - cam->pitch(); yaw = - cam->yaw(); @@ -1011,59 +1028,58 @@ GLboolean valid; glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - if( valid == true ) { - GLdouble wx, wy, wz; - GLint viewport[4]; - glGetIntegerv(GL_VIEWPORT, viewport); - - GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); - - GLdouble projection[16]; - glGetDoublev(GL_PROJECTION_MATRIX, projection); - + if( valid ) + { + GLdouble wx, wy, wz; + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + + GLdouble modelview[16]; + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + + GLdouble projection[16]; + glGetDoublev(GL_PROJECTION_MATRIX, projection); + //get width and height in world coords - gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - w = wx; - gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - h = wy; - - - // calculate speech bubble margin - const float m = h/10; - - // draw inside of bubble - PushColor( BUBBLE_FILL ); - glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); - glEnable( GL_POLYGON_OFFSET_FILL ); - glPolygonOffset( 1.0, 1.0 ); - gl_draw_octagon( w, h, m ); - glDisable( GL_POLYGON_OFFSET_FILL ); - PopColor(); - - // draw outline of bubble - PushColor( BUBBLE_BORDER ); - glLineWidth( 1 ); - glEnable( GL_LINE_SMOOTH ); - glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); - glPopAttrib(); - PopColor(); - - PushColor( BUBBLE_TEXT ); - // draw text inside the bubble - gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); - PopColor(); - - } + gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + w = wx; + gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + h = wy; + + // calculate speech bubble margin + const float m = h/10; + + // draw inside of bubble + PushColor( BUBBLE_FILL ); + glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); + glPolygonMode( GL_FRONT, GL_FILL ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 1.0, 1.0 ); + gl_draw_octagon( w, h, m ); + glDisable( GL_POLYGON_OFFSET_FILL ); + PopColor(); + + // draw outline of bubble + PushColor( BUBBLE_BORDER ); + glLineWidth( 1 ); + glEnable( GL_LINE_SMOOTH ); + glPolygonMode( GL_FRONT, GL_LINE ); + gl_draw_octagon( w, h, m ); + glPopAttrib(); + PopColor(); + + PushColor( BUBBLE_TEXT ); + // draw text inside the bubble + gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + PopColor(); + } glPopMatrix(); - } - - if( stall ) - { + } + + if( stall ) + { DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 ); - } + } } stg_meters_t StgModel::ModelHeight() @@ -1228,7 +1244,6 @@ PopCoords(); } - void StgModel::DrawGrid( void ) { if ( gui_grid ) @@ -1250,7 +1265,6 @@ } } - inline bool velocity_is_zero( stg_velocity_t& v ) { return( !(v.x || v.y || v.z || v.a) ); @@ -1291,14 +1305,6 @@ world->NeedRedraw(); } -// void StgModel::GPoseDirtyTree( void ) -// { -// this->gpose_dirty = true; // our global pose may have changed - -// for( GList* it = this->children; it; it=it->next ) -// ((StgModel*)it->data)->GPoseDirtyTree(); -// } - void StgModel::SetPose( stg_pose_t pose ) { //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/model_laser.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -319,11 +319,13 @@ glTranslatef( 0,0, geom.size.z/2.0 ); // shoot the laser beam out at the right height // DEBUG - draw the origin of the laser beams + PushColor( 0,0,0,1.0 ); glPointSize( 4.0 ); glBegin( GL_POINTS ); glVertex2f( 0,0 ); glEnd(); - + PopColor(); + // pack the laser hit points into a vertex array for fast rendering static float* pts = NULL; pts = (float*)g_realloc( pts, 2 * (sample_count+1) * sizeof(float)); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/stage.hh 2008-11-19 08:22:32 UTC (rev 7159) @@ -1384,6 +1384,8 @@ void DrawBlocksTree(); virtual void DrawBlocks(); + void DrawBoundingBox(); + void DrawBoundingBoxTree(); virtual void DrawStatus( Camera* cam ); void DrawStatusTree( Camera* cam ); @@ -1924,6 +1926,8 @@ virtual bool Save( const char* filename ); inline virtual bool IsGUI() { return true; } + + void DrawBoundingBoxTree(); void Start(){ paused = false; }; void Stop(){ paused = true; }; Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/stage_internal.hh 2008-11-19 08:22:32 UTC (rev 7159) @@ -81,39 +81,41 @@ void AddModel( StgModel* mod ); - Option - showBlinken, - showBlocks, - showClock, - showData, - showFlags, - showFollow, - showFootprints, - showGrid, - showOccupancy, - showScreenshots, - showStatus, - showTrailArrows, - showTrailRise, - showTrails, - showTree, - pCamOn, - visualizeAll; + Option showBlinken, + showBlocks, + showClock, + showData, + showFlags, + showFollow, + showFootprints, + showGrid, + showOccupancy, + showScreenshots, + showStatus, + showTrailArrows, + showTrailRise, + showTrails, + showTree, + showBBoxes, + showBlur, + pCamOn, + visualizeAll; public: - StgCanvas( StgWorldGui* world, int x, int y, int W,int H); + StgCanvas( StgWorldGui* world, int x, int y, int width, int height); ~StgCanvas(); bool graphics; StgWorldGui* world; void Screenshot(); - + void InitGl(); void createMenuItems( Fl_Menu_Bar* menu, std::string path ); void FixViewport(int W,int H); void DrawFloor(); //simpler floor compared to grid void DrawBlocks(); + void DrawBoundingBoxes(); void resetCamera(); virtual void renderFrame(); virtual void draw(); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/world.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -239,8 +239,17 @@ //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 StgWorld::Load( const char* worldfile_path ) { + // note: must call Unload() before calling Load() if a world already + // exists TODO: unload doesn't clean up enough right now + GHashTable* entitytable = g_hash_table_new( g_direct_hash, g_direct_equal ); printf( " [Loading %s]", worldfile_path ); @@ -338,11 +347,6 @@ free(mod); } -// delete a model from the hash table -static void destroy_sregion( gpointer dummy1, SuperRegion* sr, gpointer dummy2 ) -{ - free(sr); -} void StgWorld::UnLoad() { @@ -877,7 +881,6 @@ } } - void StgWorld::Extend( stg_point3_t pt ) { extent.x.min = MIN( extent.x.min, pt.x); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/libstage/worldgui.cc 2008-11-19 08:22:32 UTC (rev 7159) @@ -695,6 +695,10 @@ } } +void StgWorldGui::DrawBoundingBoxTree() +{ + LISTMETHOD( StgWorld::children, StgModel*, DrawBoundingBoxTree ); +} void StgWorldGui::PushColor( stg_color_t col ) { canvas->PushColor( col ); } Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/worlds/fasr.world 2008-11-19 08:22:32 UTC (rev 7159) @@ -62,7 +62,7 @@ define autorob pioneer2dx ( - sicklaser( samples 36 range_max 5 laser_return 2 ) + sicklaser( samples 32 range_max 5 laser_return 2 ) ctrl "fasr" ) Modified: code/stage/trunk/worlds/pioneer.inc =================================================================== --- code/stage/trunk/worlds/pioneer.inc 2008-11-19 01:24:40 UTC (rev 7158) +++ code/stage/trunk/worlds/pioneer.inc 2008-11-19 08:22:32 UTC (rev 7159) @@ -131,7 +131,7 @@ # use the sonar array defined above with a small vertical offset to # drop the sensors into the robot body - p2dx_sonar( pose [0.04 0 -0.03 0] ) + p2dx_sonar( pose [0 0 -0.03 0] ) # differential steering model drive "diff" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-26 19:44:53
|
Revision: 7171 http://playerstage.svn.sourceforge.net/playerstage/?rev=7171&view=rev Author: rtv Date: 2008-11-26 19:44:48 +0000 (Wed, 26 Nov 2008) Log Message: ----------- added quick waypoint property and vis 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/model.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-11-25 22:47:00 UTC (rev 7170) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-26 19:44:48 UTC (rev 7171) @@ -77,6 +77,20 @@ robot->sink = mod->GetWorld()->GetModel( "sink" ); assert(robot->sink); + const uint32_t wp_count = 100; + Waypoint* wps = new Waypoint[wp_count]; + + for( int i=0; i<wp_count; i++ ) + { + wps[i].pose.x = i / 12.5; + wps[i].pose.y = 4.0*drand48() - 2.0; + wps[i].pose.z = 0; + wps[i].pose.a = 0; + wps[i].color = stg_color_pack( 1,0,1,0 ); + } + + robot->pos->SetWaypoints( wps, wp_count ); + return 0; //ok } Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-11-25 22:47:00 UTC (rev 7170) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-11-26 19:44:48 UTC (rev 7171) @@ -30,6 +30,7 @@ stage.hh texture_manager.cc typetable.cc + waypoint.cc world.cc worldfile.cc worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-25 22:47:00 UTC (rev 7170) +++ code/stage/trunk/libstage/canvas.cc 2008-11-26 19:44:48 UTC (rev 7171) @@ -760,7 +760,10 @@ if( showBBoxes ) DrawBoundingBoxes(); - + + // TODO - finish this properly + LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); + // MOTION BLUR if( showBlur ) { @@ -879,7 +882,7 @@ last_selection->DataVisualizeTree( current_camera ); } } - + if( showGrid ) LISTMETHOD( models_sorted, StgModel*, DrawGrid ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-25 22:47:00 UTC (rev 7170) +++ code/stage/trunk/libstage/model.cc 2008-11-26 19:44:48 UTC (rev 7171) @@ -194,7 +194,9 @@ wf_entity(0), has_default_block( true ), map_caches_are_invalid( true ), - thread_safe( false ) + thread_safe( false ), + waypoints( NULL ), + waypoint_count( 0 ) { assert( modelsbyid ); assert( world ); @@ -1677,3 +1679,34 @@ } +/** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ +Waypoint* StgModel::SetWaypoints( Waypoint* wps, uint32_t count ) +{ + Waypoint* replaced = waypoints; + + waypoints = wps; + waypoint_count = count; + + return replaced; +} + + +void StgModel::DrawWaypoints() +{ + if( waypoints && waypoint_count ) + { + //PushLocalCoords(); + + PushColor( color ); + glPointSize( 3 ); + + // puts( "drawing wps" ); + + for( unsigned int i=0; i < waypoint_count; i++ ) + waypoints[i].Draw(); + + //PopCoords(); + // restore a sensible drawing color + PopColor(); + } +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-25 22:47:00 UTC (rev 7170) +++ code/stage/trunk/libstage/stage.hh 2008-11-26 19:44:48 UTC (rev 7171) @@ -348,7 +348,18 @@ size.y ); } }; + + class Waypoint + { + public: + Waypoint( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a, stg_color_t color ) ; + Waypoint(); + void Draw(); + stg_pose_t pose; + stg_color_t color; + }; + /** bound a range of values, from min to max */ typedef struct { @@ -868,7 +879,7 @@ static GList* world_list; ///< all the worlds that exist static bool quit_all; ///< quit all worlds ASAP static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - + stg_usec_t real_time_start; ///< the real time at which this world was created bool destroy; bool quit; ///< quit this world ASAP @@ -1054,6 +1065,7 @@ /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() { return updates; } + // stg_point_t* LocalToGlobal( double scalex, double scaley, // stg_point_t pts[], @@ -1291,7 +1303,10 @@ GMutex* access_mutex; - + Waypoint* waypoints; + uint32_t waypoint_count; + void DrawWaypoints(); + public: void Lock() { @@ -1546,6 +1561,9 @@ bool IsDescendent( StgModel* testmod ); bool IsRelated( StgModel* mod2 ); + + /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ + Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); /** get the pose of a model in the global CS */ stg_pose_t GetGlobalPose(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-28 01:21:50
|
Revision: 7181 http://playerstage.svn.sourceforge.net/playerstage/?rev=7181&view=rev Author: rtv Date: 2008-11-28 01:21:44 +0000 (Fri, 28 Nov 2008) Log Message: ----------- moved waypoints to position model only, and added coord system viz Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.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/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -27,7 +27,6 @@ }; - typedef struct { StgModelPosition* pos; @@ -77,6 +76,21 @@ robot->sink = mod->GetWorld()->GetModel( "sink" ); assert(robot->sink); + + const int waypoint_count = 100; + Waypoint* waypoints = new Waypoint[waypoint_count]; + + for( int i=0; i<waypoint_count; i++ ) + { + waypoints[i].pose.x = i* 0.1; + waypoints[i].pose.y = drand48() * 4.0; + waypoints[i].pose.z = 0; + waypoints[i].pose.a = normalize( i/10.0 ); + waypoints[i].color = stg_color_pack( 0,0,1,0 ); + } + + robot->pos->SetWaypoints( waypoints, waypoint_count ); + return 0; //ok } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/canvas.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -762,7 +762,7 @@ DrawBoundingBoxes(); // TODO - finish this properly - LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); + //LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); // MOTION BLUR if( showBlur ) Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/gl.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -9,11 +9,18 @@ } // transform the current coordinate frame by the given pose -void Stg::gl_pose_shift( stg_pose_t* pose ) +void Stg::gl_pose_shift( const stg_pose_t &pose ) { - gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); + gl_coord_shift( pose.x, pose.y, pose.z, pose.a ); } +void Stg::gl_pose_inverse_shift( const stg_pose_t &pose ) +{ + gl_coord_shift( 0,0,0, -pose.a ); + gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 ); +} + + // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { @@ -45,7 +52,21 @@ glEnd(); } +void Stg::gl_draw_vector( double x, double y, double z ) +{ + glBegin( GL_LINES ); + glVertex3f( 0,0,0 ); + glVertex3f( x,y,z ); + glEnd(); +} +void Stg::gl_draw_origin( double len ) +{ + gl_draw_vector( len,0,0 ); + gl_draw_vector( 0,len,0 ); + gl_draw_vector( 0,0,len ); +} + void Stg::gl_draw_grid( stg_bounds3d_t vol ) { glBegin(GL_LINES); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -194,9 +194,7 @@ wf_entity(0), has_default_block( true ), map_caches_are_invalid( true ), - thread_safe( false ), - waypoints( NULL ), - waypoint_count( 0 ) + thread_safe( false ) { assert( modelsbyid ); assert( world ); @@ -581,23 +579,6 @@ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } -// stg_point3_t StgModel::LocalToGlobal( stg_point3_t point ) -// { -// stg_pose_t pose; -// pose.x = point.x; -// pose.y = point.y; -// pose.z = point.z; -// pose.a = 0; - -// pose = LocalToGlobal( pose ); - -// point.x = pose.x; -// point.y = pose.y; -// point.z = pose.z; - -// return point; -// } - void StgModel::MapWithChildren() { UnMap(); @@ -617,33 +598,6 @@ ((StgModel*)it->data)->UnMapWithChildren(); } -// given an input point array in model local coordinates, return -// an array with the same points in global coordinates. caller must -// delete[] the points. -// stg_point_t* StgModel::LocalToGlobal( double scalex, -// double scaley, -// stg_point_t pts[], -// uint32_t pt_count ) -// { -// stg_point_t* glob = new stg_point_t[pt_count]; - -// stg_pose_t global_pose = GetGlobalPose(); - -// for( int p=0; p<pt_count; p++ ) -// { -// stg_pose_t local( pts[p].x * scalex, -// pts[p].y * scaley, -// 0, 0 ); -// stg_pose_t global = pose_sum( global_pose, local ); - -// glob[p].x = global.x; -// glob[p].y = global.y; -// } - -// return glob; -// } - - void StgModel::Map() { //PRINT_DEBUG1( "%s.Map()", token ); @@ -773,7 +727,7 @@ glRotatef( rtod(pose.a), 0,0,1 ); - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); double dx = geom.size.x / 2.0 * 1.6; double dy = geom.size.y / 2.0 * 1.6; @@ -804,8 +758,8 @@ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); glPushMatrix(); - gl_pose_shift( &checkpoint->pose ); - gl_pose_shift( &geom.pose ); + gl_pose_shift( checkpoint->pose ); + gl_pose_shift( geom.pose ); stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); @@ -926,7 +880,7 @@ void StgModel::DrawBlocks( ) { - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); blockgroup.CallDisplayList( this ); } @@ -940,7 +894,7 @@ void StgModel::DrawBoundingBox() { - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); PushColor( color ); @@ -985,7 +939,7 @@ if( parent ) glTranslatef( 0,0, parent->geom.size.z ); - gl_pose_shift( &pose ); + gl_pose_shift( pose ); } void StgModel::PopCoords() @@ -1677,36 +1631,3 @@ { blockgroup.UnMap(); } - - -/** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ -Waypoint* StgModel::SetWaypoints( Waypoint* wps, uint32_t count ) -{ - Waypoint* replaced = waypoints; - - waypoints = wps; - waypoint_count = count; - - return replaced; -} - - -void StgModel::DrawWaypoints() -{ - if( waypoints && waypoint_count ) - { - //PushLocalCoords(); - - PushColor( color ); - glPointSize( 3 ); - - // puts( "drawing wps" ); - - for( unsigned int i=0; i < waypoint_count; i++ ) - waypoints[i].Draw(); - - //PopCoords(); - // restore a sensible drawing color - PopColor(); - } -} Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -56,7 +56,11 @@ StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world, StgModel* parent ) - : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT ) + : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT ), + dutycycle( 1.0 ), + enabled( true ), + period( 1000 ), + on( true ) { PRINT_DEBUG2( "Constructing StgModelBlinkenlight %d (%s)\n", id, typestr ); @@ -73,10 +77,6 @@ geom.size.z = 0.02; this->SetGeom( geom ); - this->dutycycle = 1.0; - this->enabled = true; - this->period = 1000; - this->on = true; this->Startup(); @@ -108,17 +108,10 @@ void StgModelBlinkenlight::DataVisualize( Camera* cam ) { + // TODO XX if( on && showBlinkenData ) { - glPushMatrix(); - // move into this model's local coordinate frame - gl_pose_shift( &this->pose ); - gl_pose_shift( &this->geom.pose ); - glLineWidth( 3 ); - // TODO //LISTMETHOD( this->blocks, StgBlock*, Draw ); - glLineWidth( 1 ); - glPopMatrix(); // drop out of local coords } } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_laser.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -309,6 +309,8 @@ glPushMatrix(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + // we only regenerate the list if there's new data if( 1 ) // data_dirty ) // TODO - hmm, why doesn't this work? { Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_position.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -40,13 +40,13 @@ position ( # position properties - drive "" - localization "" + 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.05] + odom_error [0.03 0.03 0.00 0.05] # model properties ) @@ -84,7 +84,9 @@ StgModelPosition::StgModelPosition( StgWorld* world, StgModel* parent ) - : StgModel( world, parent, MODEL_TYPE_POSITION ) + : StgModel( world, parent, MODEL_TYPE_POSITION ), + waypoints( NULL ), + waypoint_count( 0 ) { PRINT_DEBUG2( "Constructing StgModelPosition %d (%s)\n", id, typestr ); @@ -178,14 +180,13 @@ { 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.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 2, est_origin.a ); + 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 stg_pose_t gpose = this->GetGlobalPose(); est_pose.a = normalize( gpose.a - est_origin.a ); - //double cosa = cos(est_pose.a); - //double sina = sin(est_pose.a); double cosa = cos(est_origin.a); double sina = sin(est_origin.a); double dx = gpose.x - est_origin.x; @@ -204,8 +205,10 @@ 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", 2, integration_error.a ); + = wf->ReadTupleAngle( wf_entity, "odom_error", 3, integration_error.a ); } // choose a localization model @@ -400,7 +403,6 @@ stg_pose_t gpose = this->GetGlobalPose(); est_pose.a = normalize( gpose.a - est_origin.a ); - //est_pose.a =0;// 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; @@ -425,7 +427,6 @@ est_pose.x += dx * cosa + dy * sina; est_pose.y -= dy * cosa - dx * sina; - } break; @@ -448,8 +449,6 @@ PRINT_DEBUG( "position startup" ); this->SetWatts( STG_POSITION_WATTS ); - - //stg_model_position_odom_reset( mod ); } void StgModelPosition::Shutdown( void ) @@ -544,3 +543,74 @@ est_origin.a = da; } +/** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ +Waypoint* StgModelPosition::SetWaypoints( Waypoint* wps, uint32_t count ) +{ + Waypoint* replaced = waypoints; + + waypoints = wps; + waypoint_count = count; + + return replaced; +} + +void StgModelPosition::DataVisualize( Camera* cam ) +{ + if( waypoints && waypoint_count ) + DrawWaypoints(); + + // 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 ); + 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(); + + 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(); + PopColor(); + + glPopMatrix(); +} + +void StgModelPosition::DrawWaypoints() +{ + glPointSize( 3 ); + + // TRUE + glPushMatrix(); + PushColor( color ); + + gl_pose_inverse_shift( pose ); + gl_pose_shift( est_origin ); + + for( unsigned int i=0; i < waypoint_count; i++ ) + waypoints[i].Draw(); + + 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/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_ranger.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -330,6 +330,7 @@ if( showRangerTransducers ) { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0,0,0,1 ); for( unsigned int s=0; s<sensor_count; s++ ) @@ -384,6 +385,7 @@ } // draw the filled triangles in transparent pale green + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glDepthMask( GL_FALSE ); PushColor( 0, 1, 0, 0.1 ); glVertexPointer( 3, GL_FLOAT, 0, pts ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/stage.hh 2008-11-28 01:21:44 UTC (rev 7181) @@ -615,7 +615,8 @@ void RegisterModels(); void gl_draw_grid( stg_bounds3d_t vol ); - void gl_pose_shift( stg_pose_t* pose ); + void gl_pose_shift( const stg_pose_t &pose ); + void gl_pose_inverse_shift( const stg_pose_t &pose ); void gl_coord_shift( double x, double y, double z, double a ); class StgFlag @@ -632,6 +633,8 @@ void gl_draw_string( float x, float y, float z, const char *string); void gl_speech_bubble( float x, float y, float z, const char* str ); void gl_draw_octagon( float w, float h, float m ); + void gl_draw_vector( double x, double y, double z ); + void gl_draw_origin( double len ); typedef int(*stg_model_callback_t)(StgModel* mod, void* user ); typedef int(*stg_cell_callback_t)(Cell* cell, void* user ); @@ -1303,10 +1306,6 @@ GMutex* access_mutex; - Waypoint* waypoints; - uint32_t waypoint_count; - void DrawWaypoints(); - public: void Lock() { @@ -1562,9 +1561,6 @@ bool IsRelated( StgModel* mod2 ); - /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ - Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); - /** get the pose of a model in the global CS */ stg_pose_t GetGlobalPose(); @@ -2464,12 +2460,19 @@ /// %StgModelPosition class class StgModelPosition : public StgModel { + friend class StgCanvas; + private: stg_pose_t goal; //< the current velocity or pose to reach, depending on the value of control_mode stg_position_control_mode_t control_mode; stg_position_drive_mode_t drive_mode; stg_position_localization_mode_t localization_mode; ///< global or local mode stg_velocity_t integration_error; ///< errors to apply in simple odometry model + + Waypoint* waypoints; + uint32_t waypoint_count; + void DrawWaypoints(); + public: static const char* typestr; // constructor @@ -2482,7 +2485,12 @@ virtual void Shutdown(); 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 ); + /** Set the current pose estimate.*/ void SetOdom( stg_pose_t odom ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/worlds/fasr.world 2008-11-28 01:21:44 UTC (rev 7181) @@ -7,8 +7,8 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds -paused 0 +interval_real 50 # real-time interval between simulation updates in milliseconds +paused 1 resolution 0.02 @@ -29,7 +29,7 @@ pcam_angle [ 70.000 0 ] pcam_on 0 - show_data 0 + show_data 1 show_flags 1 interval 50 @@ -64,29 +64,32 @@ ( sicklaser( samples 32 range_max 5 laser_return 2 ) ctrl "fasr" + + localization "odom" ) autorob( pose [5.099 4.804 0 -73.937] ) -autorob( pose [6.471 5.304 0 14.941] ) -autorob( pose [5.937 4.858 0 -147.503] ) -autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.664 5.938 0 -51.799] ) -autorob( pose [7.016 6.428 0 -128.279] ) -autorob( pose [5.911 4.040 0 -97.047] ) -autorob( pose [4.909 6.097 0 -44.366] ) -autorob( pose [6.898 4.775 0 -117.576] ) -autorob( pose [7.012 5.706 0 129.497] ) +#autorob( pose [5.099 4.804 0 -73.937] localization_origin [5.099 4.804 0 -73.937] ) +#autorob( pose [6.471 5.304 0 14.941] ) +#autorob( pose [5.937 4.858 0 -147.503] ) +#autorob( pose [7.574 6.269 0 -111.715] ) +#autorob( pose [5.664 5.938 0 -51.799] ) +#autorob( pose [7.016 6.428 0 -128.279] ) +#autorob( pose [5.911 4.040 0 -97.047] ) +#autorob( pose [4.909 6.097 0 -44.366] ) +#autorob( pose [6.898 4.775 0 -117.576] ) +#autorob( pose [7.012 5.706 0 129.497] ) -autorob( pose [6.616 6.893 0 170.743] ) -autorob( pose [6.451 4.189 0 -61.453] ) -autorob( pose [5.098 6.788 0 -61.295] ) -autorob( pose [4.374 5.163 0 -90.417] ) -autorob( pose [4.999 4.230 0 -42.157] ) -autorob( pose [4.331 4.217 0 -95.000] ) -autorob( pose [5.440 5.317 0 -26.545] ) -autorob( pose [7.518 6.973 0 163.239] ) -autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [4.839 3.595 0 -179.567] ) +#autorob( pose [6.616 6.893 0 170.743] ) +#autorob( pose [6.451 4.189 0 -61.453] ) +#autorob( pose [5.098 6.788 0 -61.295] ) +#autorob( pose [4.374 5.163 0 -90.417] ) +#autorob( pose [4.999 4.230 0 -42.157] ) +#autorob( pose [4.331 4.217 0 -95.000] ) +#autorob( pose [5.440 5.317 0 -26.545] ) +#autorob( pose [7.518 6.973 0 163.239] ) +#autorob( pose [7.559 4.764 0 -139.066] ) +#autorob( pose [4.839 3.595 0 -179.567] ) #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [6.203 6.963 0 2.937] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-02 01:45:49
|
Revision: 7184 http://playerstage.svn.sourceforge.net/playerstage/?rev=7184&view=rev Author: rtv Date: 2008-12-02 01:45:44 +0000 (Tue, 02 Dec 2008) Log Message: ----------- geometry & graphics fixes. added normalization of blocks to fit inside model geometry Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc 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 code/stage/trunk/worlds/walle.inc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/block.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -186,9 +186,11 @@ g_ptr_array_set_size( candidate_cells, 0 ); // compute the global location of the first point - stg_pose_t local( pts[0].x * scale.x, - pts[0].y * scale.y, - 0, 0 ); + stg_pose_t local( (pts[0].x - mod->blockgroup.offset.x) * scale.x , + (pts[0].y - mod->blockgroup.offset.y) * scale.y, + -mod->blockgroup.offset.z, + 0 ); + stg_pose_t first_gpose, last_gpose; first_gpose = last_gpose = pose_sum( gpose, local ); @@ -199,9 +201,10 @@ // now loop from the the second to the last for( int p=1; p<pt_count; p++ ) { - stg_pose_t local( pts[p].x * scale.x, - pts[p].y * scale.y, - 0, 0 ); + stg_pose_t local( (pts[p].x - mod->blockgroup.offset.x) * scale.x , + (pts[p].y - mod->blockgroup.offset.y) * scale.y, + -mod->blockgroup.offset.z, + 0 ); stg_pose_t gpose2 = pose_sum( gpose, local ); @@ -325,6 +328,8 @@ color = stg_lookup_color( colorstr ); inherit_color = false; } + else + inherit_color = true; } Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/blockgroup.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -91,9 +91,17 @@ size.z = MAX( block->local_z.max, size.z ); } - // store these bounds for scaling purposes + // store these bounds for normalization purposes size.x = maxx-minx; size.y = maxy-miny; + + 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 ) + //((StgBlock*)it->data)->Normalize( size.x, size.y, size.z, offset.x } @@ -107,14 +115,36 @@ LISTMETHOD( blocks, StgBlock*, UnMap ); } -void BlockGroup::DrawSolid() +void BlockGroup::DrawSolid( const stg_geom_t & geom ) { + glPushMatrix(); + + gl_pose_shift( geom.pose ); + + glScalef( geom.size.x / size.x, + geom.size.y / size.y, + geom.size.z / size.z ); + + glTranslatef( -offset.x, -offset.y, -offset.z ); + LISTMETHOD( blocks, StgBlock*, DrawSolid ); + + glPopMatrix(); } -void BlockGroup::DrawFootPrint() +void BlockGroup::DrawFootPrint( const stg_geom_t & geom ) { + glPushMatrix(); + + glScalef( geom.size.x / size.x, + geom.size.y / size.y, + geom.size.z / size.z ); + + glTranslatef( -offset.x, -offset.y, -offset.z ); + LISTMETHOD( blocks, StgBlock*, DrawFootPrint); + + glPopMatrix(); } void BlockGroup::BuildDisplayList( StgModel* mod ) @@ -126,11 +156,17 @@ glNewList( displaylist, GL_COMPILE ); + stg_geom_t geom = mod->GetGeom(); + + gl_pose_shift( geom.pose ); + glScalef( geom.size.x / size.x, geom.size.y / size.y, geom.size.z / size.z ); + glTranslatef( -offset.x, -offset.y, -offset.z ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1.0, 1.0); Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/canvas.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -176,6 +176,7 @@ current_camera->SetProjection(); current_camera->Draw(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glDisable(GL_DITHER); glDisable(GL_BLEND); // turns off alpha blending, so we read back // exactly what we write to a pixel @@ -502,6 +503,9 @@ Fl::remove_timeout( (Fl_Timeout_Handler)StgCanvas::TimerCallback ); } + redraw(); // in case something happened that will never be + // drawn 'cos we cancelled the timeout + break; case ' ': // space bar Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/gl.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -24,11 +24,8 @@ // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { - //const char *c; glRasterPos3f( x, y, z ); gl_draw(str); -// for (c=str; *c != '\0'; c++) -// glutBitmapCharacter( GLUT_BITMAP_HELVETICA_12, *c); } void Stg::gl_speech_bubble( float x, float y, float z, const char* str ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/model.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -764,12 +764,12 @@ stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); - blockgroup.DrawFootPrint(); + blockgroup.DrawFootPrint( geom ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); PushColor( r/2, g/2, b/2, 0.1 ); - blockgroup.DrawFootPrint(); + blockgroup.DrawFootPrint( geom ); PopColor(); PopColor(); @@ -880,7 +880,6 @@ void StgModel::DrawBlocks( ) { - gl_pose_shift( geom.pose ); blockgroup.CallDisplayList( this ); } @@ -1176,7 +1175,7 @@ PushLocalCoords(); // draw the boxes - blockgroup.DrawSolid(); + blockgroup.DrawSolid( geom ); // recursively draw the tree below this model LISTMETHOD( this->children, StgModel*, DrawPicker ); Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/model_position.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -568,6 +568,27 @@ 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(); + + 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 ); + + 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 ); @@ -585,7 +606,7 @@ PushColor( r, g, b, 0.5 ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - blockgroup.DrawFootPrint(); + blockgroup.DrawFootPrint( geom ); PopColor(); glPopMatrix(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/stage.hh 2008-12-02 01:45:44 UTC (rev 7184) @@ -1182,7 +1182,8 @@ GList* blocks; uint32_t count; stg_size_t size; - + stg_point3_t offset; + BlockGroup(); ~BlockGroup(); @@ -1198,11 +1199,14 @@ StgModel* TestCollision(); void SwitchToTestedCells(); - void Map(); + void Map(); void UnMap(); - - void DrawSolid(); // draw the block in OpenGL as a solid single color - void DrawFootPrint(); // draw the projection of the block onto the z=0 plane + + void DrawSolid( const stg_geom_t &geom); // draw the block in OpenGL as a solid single color + void DrawFootPrint( const stg_geom_t &geom); // draw the + // projection of the + // block onto the z=0 + // plane void LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile *wf ); void LoadBlock( StgModel* mod, Worldfile* wf, int entity ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/libstage/world.cc 2008-12-02 01:45:44 UTC (rev 7184) @@ -46,7 +46,6 @@ #include <limits.h> #include <libgen.h> // for dirname(3) #include "stage_internal.hh" -//#include "region.hh" #include "file_manager.hh" // static data members Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/worlds/simple.world 2008-12-02 01:45:44 UTC (rev 7184) @@ -8,9 +8,9 @@ include "walle.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 10 # real-time interval between simulation updates in milliseconds -paused 1 +paused 0 resolution 0.02 @@ -40,6 +40,8 @@ # can refer to the robot by this name name "r0" + localization "odom" + pose [ 0.892 0.800 0 56.500 ] sicklaser( samples 180 ) Modified: code/stage/trunk/worlds/walle.inc =================================================================== --- code/stage/trunk/worlds/walle.inc 2008-12-01 21:30:59 UTC (rev 7183) +++ code/stage/trunk/worlds/walle.inc 2008-12-02 01:45:44 UTC (rev 7184) @@ -7,7 +7,7 @@ define walle position ( - color "yellow" + color "gray50" drive "diff" # Differential steering model. obstacle_return 1 # Can hit things. @@ -18,8 +18,8 @@ localization "gps" - size [0.5 0.5 0.5] - origin [-0.25 -0.25 0 0 ] + size [0.35 0.5 0.5] + #origin [-0.25 -0.25 0 0 ] laser( pose [0.15 0 -0.27 0 ] size [0.01 0.01 0.01] alwayson 1 ) @@ -32,7 +32,7 @@ point[2] [ 0.9 0.75 ] point[3] [ 0.1 0.75 ] z [ 0.2 0.6 ] - color "yellow" + #color "yellow" ) # body top @@ -44,7 +44,6 @@ point[2] [ 0.9 0.75 ] point[3] [ 0.1 0.75 ] z [ 0.6 0.7 ] - color "gray20" ) # right wheel @@ -56,7 +55,6 @@ point[2] [ 0.0 0.25 ] point[3] [ 0 0.0 ] z [ 0.0 0.4 ] - color "gray23" ) # right wheel @@ -68,7 +66,6 @@ point[2] [ 0 1 ] point[3] [ 0 0.75 ] z [ 0.0 0.4 ] - color "gray23" ) # right eye front @@ -80,7 +77,6 @@ point[2] [0.6 0.49] point[3] [0.6 0.30] z [0.8 0.95] - color "gray23" ) # left eye front @@ -92,7 +88,6 @@ point[2] [0.6 0.7] point[3] [0.6 0.51] z [0.8 0.95] - color "gray23" ) # right eye rear @@ -104,7 +99,6 @@ point[2] [0.25 0.49] point[3] [0.25 0.34] z [0.82 0.93] - color "gray28" ) # neck @@ -115,7 +109,6 @@ point[1] [0.4 0.55] point[0] [0.4 0.45] z [0.7 0.9] - color "gray28" ) # right eye rear @@ -127,7 +120,6 @@ point[2] [0.25 0.66] point[3] [0.25 0.51] z [0.82 0.93] - color "gray28" ) # right arm @@ -139,7 +131,6 @@ point[2] [0.35 0.25] point[3] [0.35 0.18] z [0.55 0.65] - color "gray28" ) # left arm @@ -151,7 +142,6 @@ point[2] [1.1 0.75] point[3] [1.1 0.82] z [0.55 0.65] - color "gray28" ) # left hand right finger @@ -163,7 +153,6 @@ point[2] [1.2 0.70] point[3] [1.2 0.78] z [0.5 0.65] - color "gray28" ) # left hand left finger @@ -175,7 +164,6 @@ point[2] [1.2 0.79] point[3] [1.2 0.87] z [0.5 0.65] - color "gray28" ) # right hand right finger @@ -187,7 +175,6 @@ point[2] [1.2 0.13] point[3] [1.2 0.21] z [0.5 0.65] - color "gray28" ) # right hand left finger @@ -199,7 +186,6 @@ point[2] [1.2 0.22] point[3] [1.2 0.30] z [0.5 0.65] - color "gray28" ) ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-02 02:30:09
|
Revision: 7185 http://playerstage.svn.sourceforge.net/playerstage/?rev=7185&view=rev Author: rtv Date: 2008-12-02 02:30:04 +0000 (Tue, 02 Dec 2008) Log Message: ----------- gui clean up Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.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/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -77,19 +77,19 @@ assert(robot->sink); - const int waypoint_count = 100; - Waypoint* waypoints = new Waypoint[waypoint_count]; +// const int waypoint_count = 100; +// Waypoint* waypoints = new Waypoint[waypoint_count]; - for( int i=0; i<waypoint_count; i++ ) - { - waypoints[i].pose.x = i* 0.1; - waypoints[i].pose.y = drand48() * 4.0; - waypoints[i].pose.z = 0; - waypoints[i].pose.a = normalize( i/10.0 ); - waypoints[i].color = stg_color_pack( 0,0,1,0 ); - } +// for( int i=0; i<waypoint_count; i++ ) +// { +// waypoints[i].pose.x = i* 0.1; +// waypoints[i].pose.y = drand48() * 4.0; +// waypoints[i].pose.z = 0; +// waypoints[i].pose.a = normalize( i/10.0 ); +// waypoints[i].color = stg_color_pack( 0,0,1,0 ); +// } - robot->pos->SetWaypoints( waypoints, waypoint_count ); +// robot->pos->SetWaypoints( waypoints, waypoint_count ); return 0; //ok } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/model_laser.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -28,8 +28,8 @@ static const unsigned int DEFAULT_RESOLUTION = 1; static const char* DEFAULT_COLOR = "blue"; -Option StgModelLaser::showLaserData( "Show Laser Data", "show_laser", "", true ); -Option StgModelLaser::showLaserStrikes( "Show Laser Data", "show_laser_strikes", "", false ); +Option StgModelLaser::showLaserData( "Laser scans", "show_laser", "", true ); +Option StgModelLaser::showLaserStrikes( "Laser strikes", "show_laser_strikes", "", false ); /** @ingroup model @@ -334,32 +334,35 @@ pts[0] = 0.0; pts[1] = 0.0; - - PushColor( 0, 0, 1, 0.5 ); - 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( showLaserData ) + { + PushColor( 0, 0, 1, 0.5 ); - // if the sample is unusually bright, draw a little blob - if( samples[s].reflectance > 0 ) + for( unsigned int s=0; s<sample_count; s++ ) { - glBegin( GL_POINTS ); - glVertex2f( pts[2*s+2], pts[2*s+3] ); - glEnd(); - } + 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( samples[s].reflectance > 0 ) + { + glBegin( GL_POINTS ); + glVertex2f( pts[2*s+2], pts[2*s+3] ); + glEnd(); + } + } + PopColor(); + + glDepthMask( GL_FALSE ); + + // draw the filled polygon in transparent blue + PushColor( 0, 0, 1, 0.1 ); + glVertexPointer( 2, GL_FLOAT, 0, pts ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + PopColor(); } - PopColor(); - - glDepthMask( GL_FALSE ); - - // draw the filled polygon in transparent blue - PushColor( 0, 0, 1, 0.1 ); - glVertexPointer( 2, GL_FLOAT, 0, pts ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - PopColor(); if( showLaserStrikes ) { Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/model_position.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -82,6 +82,10 @@ 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 StgModelPosition::showCoords( "Position Coordinates", "show_coords", "", false ); +Option StgModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", false ); + + StgModelPosition::StgModelPosition( StgWorld* world, StgModel* parent ) : StgModel( world, parent, MODEL_TYPE_POSITION ), @@ -126,6 +130,8 @@ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A - STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0; + registerOption( &showCoords ); + registerOption( &showWaypoints ); } @@ -556,60 +562,63 @@ void StgModelPosition::DataVisualize( Camera* cam ) { - if( waypoints && waypoint_count ) + if( showWaypoints && waypoints && waypoint_count ) DrawWaypoints(); - - // vizualize my estimated pose - glPushMatrix(); - // back into global coords - gl_pose_inverse_shift( GetGlobalPose() ); + if( showCoords ) + { + // 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 ); + 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(); + PopColor(); - gl_pose_shift( est_pose ); - PushColor( 0,1,0,1 ); // pose in green - gl_draw_origin( 0.5 ); - 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(); + } } void StgModelPosition::DrawWaypoints() Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/model_ranger.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -95,8 +95,8 @@ static const char RANGER_CONFIG_COLOR[] = "gray90"; static const char RANGER_GEOM_COLOR[] = "orange"; -Option StgModelRanger::showRangerData( "Show Ranger Data", "show_ranger", "", true ); -Option StgModelRanger::showRangerTransducers( "Show Ranger transducer locations", "show_ranger_transducers", "", false ); +Option StgModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true ); +Option StgModelRanger::showRangerTransducers( "Ranger transducers", "show_ranger_transducers", "", false ); StgModelRanger::StgModelRanger( StgWorld* world, Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/stage.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -236,14 +236,14 @@ static void pb_set_pixel( Fl_Shared_Image* pb, int x, int y, uint8_t val ) { // bounds checking - int width = pb->w();//gdk_pixbuf_get_width(pb); - int height = pb->h();//gdk_pixbuf_get_height(pb); + int width = pb->w(); + int height = pb->h(); if( x >=0 && x < width && y >= 0 && y < height ) { // zeroing guchar* pix = pb_get_pixel( pb, x, y ); - unsigned int bytes_per_sample = 1;//gdk_pixbuf_get_bits_per_sample (pb) / 8; - unsigned int num_samples = pb->d();//gdk_pixbuf_get_n_channels(pb); + unsigned int bytes_per_sample = 1; + unsigned int num_samples = pb->d(); memset( pix, val, num_samples * bytes_per_sample ); } else @@ -253,8 +253,8 @@ // set all the pixels in a rectangle static void pb_set_rect( Fl_Shared_Image* pb, int x, int y, int width, int height, uint8_t val ) { - int bytes_per_sample = 1;//gdk_pixbuf_get_bits_per_sample (pb) / 8; - int num_samples = pb->d();//gdk_pixbuf_get_n_channels(pb); + int bytes_per_sample = 1; + int num_samples = pb->d(); int a, b; for( a = y; a < y+height; a++ ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/stage.hh 2008-12-02 02:30:04 UTC (rev 7185) @@ -2477,6 +2477,10 @@ uint32_t waypoint_count; void DrawWaypoints(); + private: + static Option showCoords; + static Option showWaypoints; + public: static const char* typestr; // constructor Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/libstage/worldgui.cc 2008-12-02 02:30:04 UTC (rev 7185) @@ -497,22 +497,25 @@ } -void StgWorldGui::viewOptionsCb( Fl_Widget* w, void* p ) { +void StgWorldGui::viewOptionsCb( Fl_Widget* w, void* p ) +{ StgWorldGui* worldGui = static_cast<StgWorldGui*>( p ); - - if ( !worldGui->oDlg ) { - int x = worldGui->w()+worldGui->x() + 10; - int y = worldGui->y(); - OptionsDlg* oDlg = new OptionsDlg( x,y, 180,250 ); - oDlg->callback( optionsDlgCb, worldGui ); - oDlg->setOptions( worldGui->drawOptions ); - oDlg->showAllOpt( &worldGui->canvas->visualizeAll ); - worldGui->oDlg = oDlg; - oDlg->show(); - } - else { - worldGui->oDlg->show(); // bring it to front - } + + if ( !worldGui->oDlg ) + { + int x = worldGui->w()+worldGui->x() + 10; + int y = worldGui->y(); + OptionsDlg* oDlg = new OptionsDlg( x,y, 180,250 ); + oDlg->callback( optionsDlgCb, worldGui ); + oDlg->setOptions( worldGui->drawOptions ); + oDlg->showAllOpt( &worldGui->canvas->visualizeAll ); + worldGui->oDlg = oDlg; + oDlg->show(); + } + else + { + worldGui->oDlg->show(); // bring it to front + } } void StgWorldGui::optionsDlgCb( Fl_Widget* w, void* p ) { @@ -592,7 +595,6 @@ Fl_Box* box = new Fl_Box( Spc, Spc, Width-2*Spc, pngH ); // widget that will contain image - std::string fullpath; fullpath = FileManager::findFile( "assets/stagelogo.png" ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/worlds/fasr.world 2008-12-02 02:30:04 UTC (rev 7185) @@ -65,8 +65,6 @@ ( sicklaser( samples 32 range_max 5 laser_return 2 ) ctrl "fasr" - - localization "odom" ) autorob( pose [5.099 4.804 0 -73.937] ) @@ -80,16 +78,16 @@ autorob( pose [6.898 4.775 0 -117.576] ) autorob( pose [7.012 5.706 0 129.497] ) -#autorob( pose [6.616 6.893 0 170.743] ) -#autorob( pose [6.451 4.189 0 -61.453] ) -#autorob( pose [5.098 6.788 0 -61.295] ) -#autorob( pose [4.374 5.163 0 -90.417] ) -#autorob( pose [4.999 4.230 0 -42.157] ) -#autorob( pose [4.331 4.217 0 -95.000] ) -#autorob( pose [5.440 5.317 0 -26.545] ) -#autorob( pose [7.518 6.973 0 163.239] ) -#autorob( pose [7.559 4.764 0 -139.066] ) -#autorob( pose [4.839 3.595 0 -179.567] ) +autorob( pose [6.616 6.893 0 170.743] ) +autorob( pose [6.451 4.189 0 -61.453] ) +autorob( pose [5.098 6.788 0 -61.295] ) +autorob( pose [4.374 5.163 0 -90.417] ) +autorob( pose [4.999 4.230 0 -42.157] ) +autorob( pose [4.331 4.217 0 -95.000] ) +autorob( pose [5.440 5.317 0 -26.545] ) +autorob( pose [7.518 6.973 0 163.239] ) +autorob( pose [7.559 4.764 0 -139.066] ) +autorob( pose [4.839 3.595 0 -179.567] ) #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [6.203 6.963 0 2.937] ) Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2008-12-02 01:45:44 UTC (rev 7184) +++ code/stage/trunk/worlds/simple.world 2008-12-02 02:30:04 UTC (rev 7185) @@ -10,7 +10,7 @@ interval_sim 100 # simulation timestep in milliseconds interval_real 10 # real-time interval between simulation updates in milliseconds -paused 0 +paused 1 resolution 0.02 @@ -48,4 +48,3 @@ ctrl "wander" ) -walle( pose [ 3 3 0 ] ctrl "wander" ) \ No newline at end of file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-05 22:31:20
|
Revision: 7189 http://playerstage.svn.sourceforge.net/playerstage/?rev=7189&view=rev Author: rtv Date: 2008-12-05 22:31:14 +0000 (Fri, 05 Dec 2008) Log Message: ----------- code tidying - eliminated all -Wall warnings 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/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/region.cc 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/expand.cc code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/simple.world code/stage/trunk/worlds/walle.inc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -61,7 +61,7 @@ robot->ranger = (StgModelRanger*)mod->GetModel( "ranger:0" ); assert( robot->ranger ); - robot->ranger->Subscribe(); + //robot->ranger->Subscribe(); robot->avoidcount = 0; Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/block.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -39,8 +39,8 @@ Worldfile* wf, int entity) : mod( mod ), + pt_count(0), pts(NULL), - pt_count(0), color(0), inherit_color(true), rendered_cells( g_ptr_array_sized_new(32) ), @@ -86,7 +86,7 @@ GenerateCandidateCells(); // for every cell we may be rendered into - for( int i=0; i<candidate_cells->len; i++ ) + for( unsigned int i=0; i<candidate_cells->len; i++ ) { Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i); @@ -178,17 +178,21 @@ // add local offset gpose = pose_sum( gpose, mod->geom.pose ); + stg_size_t bgsize = mod->blockgroup.GetSize(); + stg_point3_t bgoffset = mod->blockgroup.GetOffset(); + stg_point3_t scale; - scale.x = mod->geom.size.x / mod->blockgroup.size.x; - scale.y = mod->geom.size.y / mod->blockgroup.size.y; - scale.z = mod->geom.size.z / mod->blockgroup.size.z; + scale.x = mod->geom.size.x / bgsize.x; + scale.y = mod->geom.size.y / bgsize.y; + scale.z = mod->geom.size.z / bgsize.z; + g_ptr_array_set_size( candidate_cells, 0 ); // compute the global location of the first point - stg_pose_t local( (pts[0].x - mod->blockgroup.offset.x) * scale.x , - (pts[0].y - mod->blockgroup.offset.y) * scale.y, - -mod->blockgroup.offset.z, + stg_pose_t local( (pts[0].x - bgoffset.x) * scale.x , + (pts[0].y - bgoffset.y) * scale.y, + -bgoffset.z, 0 ); stg_pose_t first_gpose, last_gpose; @@ -199,11 +203,11 @@ global_z.max = (scale.z * local_z.max) + last_gpose.z; // now loop from the the second to the last - for( int p=1; p<pt_count; p++ ) + for( unsigned int p=1; p<pt_count; p++ ) { - stg_pose_t local( (pts[p].x - mod->blockgroup.offset.x) * scale.x , - (pts[p].y - mod->blockgroup.offset.y) * scale.y, - -mod->blockgroup.offset.z, + stg_pose_t local( (pts[p].x - bgoffset.x) * scale.x , + (pts[p].y - bgoffset.y) * scale.y, + -bgoffset.z, 0 ); stg_pose_t gpose2 = pose_sum( gpose, local ); @@ -231,7 +235,7 @@ // draw the top of the block - a polygon at the highest vertical // extent glBegin( GL_POLYGON); - for( int i=0; i<pt_count; i++ ) + for( unsigned int i=0; i<pt_count; i++ ) glVertex3f( pts[i].x, pts[i].y, local_z.max ); glEnd(); } @@ -311,8 +315,7 @@ // pt_count ); char key[128]; - int p; - for( p=0; p<pt_count; p++ ) { + for( unsigned int p=0; p<pt_count; p++ ) { snprintf(key, sizeof(key), "point[%d]", p ); pts[p].x = wf->ReadTupleLength(entity, key, 0, 0); Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/blockgroup.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -7,9 +7,13 @@ using namespace Stg; BlockGroup::BlockGroup() - : blocks(NULL), + : displaylist(0), + blocks(NULL), count(0), - displaylist(0) + minx(0), + maxx(0), + miny(0), + maxy(0) { /* empty */ } BlockGroup::~BlockGroup() @@ -51,7 +55,7 @@ StgModel* hitmod = NULL; for( GList* it=blocks; it; it = it->next ) - if( hitmod = ((StgBlock*)it->data)->TestCollision()) + if( (hitmod = ((StgBlock*)it->data)->TestCollision())) break; // bail on the earliest collision //printf( "blockgroup %p test collision done.\n", this ); @@ -65,7 +69,7 @@ void BlockGroup::CalcSize() { // assuming the blocks currently fit in a square +/- one billion units - double minx, miny, maxx, maxy, minz, maxz; + //double minx, miny, maxx, maxy, minz, maxz; minx = miny = billion; maxx = maxy = -billion; @@ -152,8 +156,11 @@ //printf( "display list for model %s\n", mod->token ); if( displaylist == 0 ) - displaylist = glGenLists(1); - + { + displaylist = glGenLists(1); + CalcSize(); + } + glNewList( displaylist, GL_COMPILE ); @@ -271,8 +278,8 @@ 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; + //double dx = width/2.0; + //double dy = height/2.0; //puts( "loading rects" ); for( unsigned int r=0; r<rect_count; r++ ) @@ -284,14 +291,23 @@ 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 - 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; + pts[1].y = y; + pts[2].x = x + w; + pts[2].y = y + h; + pts[3].x = x; + pts[3].y = y + h; // TODO fix this stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/canvas.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -49,10 +49,15 @@ int x, int y, int width, int height) : Fl_Gl_Window( x, y, width, height ), + wf( NULL ), + startx( -1 ), + starty( -1 ), + selected_models( NULL ), + last_selection( NULL ), + interval( 50 ), //msec between redraws // initialize Option objects showBlinken( "Blinkenlights", "show_blinkenlights", "", true ), showBlocks( "Blocks", "show_blocks", "b", true ), - showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false ), showClock( "Clock", "show_clock", "c", true ), showData( "Data", "show_data", "d", false ), showFlags( "Flags", "show_flags", "l", true ), @@ -66,18 +71,13 @@ showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false ), showTrails( "Trails/Fast", "show_trailfast", "^f", false ), showTree( "Debug/Tree", "show_tree", "^t", false ), + showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false ), showBlur( "Trails/Blur", "show_trailblur", "^d", false ), pCamOn( "Perspective camera", "pcam_on", "r", false ), visualizeAll( "Visualize All", "vis_all", "^v", true ), - // and the rest - world( world ), - selected_models( NULL ), - last_selection( NULL ), - wf( NULL ), - startx( -1 ), - starty( -1 ), - interval( 50 ), //msec between redraws - graphics( true ) + // and the rest + graphics( true ), + world( world ) { end(); @@ -524,6 +524,27 @@ } redraw(); break; + + case '[': // slow down + if( world->interval_real == 0 ) + world->interval_real = 10; + else + { + world->interval_real *= 1.2; + } + break; // need the parens above + + case ']': // speed up + if( world->interval_real == 0 ) + putchar( 7 ); // bell! + else + { + world->interval_real *= 0.8; + if( world->interval_real < 10 ) + world->interval_real = 0; + } + break; + case FL_Left: if( pCamOn == false ) { camera.move( -10, 0 ); } else { perspective_camera.strafe( -0.5 ); } break; Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/model.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -117,29 +117,7 @@ //#define DEBUG 0 #include "stage_internal.hh" #include "texture_manager.hh" -//#include <limits.h> - -//static const members -static const bool DEFAULT_BOUNDARY = false; -static const stg_color_t DEFAULT_COLOR = (0xFFFF0000); // solid red -static const stg_joules_t DEFAULT_ENERGY_CAPACITY = 1000.0; -static const bool DEFAULT_ENERGY_CHARGEENABLE = true; -static const stg_watts_t DEFAULT_ENERGY_GIVERATE = 0.0; -static const stg_meters_t DEFAULT_ENERGY_PROBERANGE = 0.0; -static const stg_watts_t DEFAULT_ENERGY_TRICKLERATE = 0.1; -static const bool DEFAULT_GRID = false; -static const bool DEFAULT_GRIPPERRETURN = false; -static const stg_laser_return_t DEFAULT_LASERRETURN = LaserVisible; -static const stg_meters_t DEFAULT_MAP_RESOLUTION = 0.1; -static const stg_movemask_t DEFAULT_MASK = (STG_MOVE_TRANS | STG_MOVE_ROT); -static const stg_kg_t DEFAULT_MASS = 10.0; -static const bool DEFAULT_NOSE = false; -static const bool DEFAULT_OBSTACLERETURN = true; -static const bool DEFAULT_BLOBRETURN = true; -static const bool DEFAULT_OUTLINE = true; -static const bool DEFAULT_RANGERRETURN = true; - // speech bubble colors static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black @@ -154,47 +132,59 @@ StgModel::StgModel( StgWorld* world, StgModel* parent, const stg_model_type_t type ) - : StgAncestor(), - world(world), - parent(parent), - type(type), - id( StgModel::count++ ), - trail( g_array_new( false, false, sizeof(stg_trail_item_t) )), + : StgAncestor(), + blinkenlights( g_ptr_array_new() ), + blob_return(true), + blockgroup(), blocks_dl(0), + boundary(false), + callbacks( g_hash_table_new( g_int_hash, g_int_equal ) ), + color( 0xFFFF0000 ), // red data_fresh(false), disabled(false), + fiducial_key(0), + fiducial_return(0), + flag_list(NULL), + geom(), + gripper_return(false), + gui_grid(false), + gui_mask( parent ? 0 : (STG_MOVE_TRANS | STG_MOVE_ROT) ), + gui_nose(false), + gui_outline(false), + has_default_block( true ), + hook_load(0), + hook_save(0), + hook_shutdown(0), + hook_startup(0), + hook_update(0), + id( StgModel::count++ ), + initfunc(NULL), + interval((stg_usec_t)1e4), // 10msec + laser_return(LaserVisible), + last_update(0), + map_caches_are_invalid( true ), + map_resolution(0.1), + mass(0), + obstacle_return(true), + on_update_list( false ), + on_velocity_list( false ), + parent(parent), + pose(), + props(NULL), + ranger_return(true), rebuild_displaylist(true), say_string(NULL), + stall(false), subs(0), + thread_safe( false ), + trail( g_array_new( false, false, sizeof(stg_trail_item_t) )), + type(type), used(false), - stall(false), - obstacle_return(DEFAULT_OBSTACLERETURN), - ranger_return(DEFAULT_RANGERRETURN), - blob_return(DEFAULT_BLOBRETURN), - laser_return(DEFAULT_LASERRETURN), - gripper_return(DEFAULT_GRIPPERRETURN), - fiducial_return(0), - fiducial_key(0), - boundary(DEFAULT_BOUNDARY), - color(DEFAULT_COLOR), - map_resolution(DEFAULT_MAP_RESOLUTION), - gui_nose(DEFAULT_NOSE), - gui_grid(DEFAULT_GRID), - gui_outline(DEFAULT_OUTLINE), - gui_mask( parent ? 0 : DEFAULT_MASK), - callbacks( g_hash_table_new( g_int_hash, g_int_equal ) ), - flag_list(NULL), - blinkenlights( g_ptr_array_new() ), - last_update(0), - interval((stg_usec_t)1e4), // 10msec - initfunc(NULL), + velocity(), + watts(0), wf(NULL), - on_velocity_list( false ), - on_update_list( false ), wf_entity(0), - has_default_block( true ), - map_caches_are_invalid( true ), - thread_safe( false ) + world(world) { assert( modelsbyid ); assert( world ); @@ -678,7 +668,7 @@ StartUpdating(); - CallCallbacks( &startup_hook ); + CallCallbacks( &hook_startup ); } void StgModel::Shutdown( void ) @@ -687,7 +677,7 @@ StopUpdating(); - CallCallbacks( &shutdown_hook ); + CallCallbacks( &hook_shutdown ); } void StgModel::UpdateIfDue( void ) @@ -706,7 +696,7 @@ // printf( "[%llu] %s update (%d subs)\n", // this->world->sim_time, this->token, this->subs ); - CallCallbacks( &update_hook ); + CallCallbacks( &hook_update ); last_update = world->sim_time; } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/model_laser.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -75,13 +75,14 @@ StgModelLaser::StgModelLaser( StgWorld* world, StgModel* parent ) : StgModel( 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_min( DEFAULT_MINRANGE ), range_max( DEFAULT_MAXRANGE ), fov( DEFAULT_FOV ), - sample_count( DEFAULT_SAMPLES ), - resolution( DEFAULT_RESOLUTION ), - data_dirty( true ), - samples( NULL ) // don't allocate sample buffer memory until Update() is called + resolution( DEFAULT_RESOLUTION ) { PRINT_DEBUG2( "Constructing StgModelLaser %d (%s)\n", @@ -334,41 +335,42 @@ pts[0] = 0.0; pts[1] = 0.0; - - if( showLaserData ) - { - PushColor( 0, 0, 1, 0.5 ); + + 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++ ) + // if the sample is unusually bright, draw a little blob + if( showLaserData && (samples[s].reflectance > 0) ) { - 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( samples[s].reflectance > 0 ) - { - glBegin( GL_POINTS ); - glVertex2f( pts[2*s+2], pts[2*s+3] ); - glEnd(); - } - } - PopColor(); - - glDepthMask( GL_FALSE ); - + glBegin( GL_POINTS ); + glVertex2f( pts[2*s+2], pts[2*s+3] ); + glEnd(); + } + } + + glVertexPointer( 2, GL_FLOAT, 0, pts ); + + PopColor(); + + if( showLaserData ) + { // draw the filled polygon in transparent blue PushColor( 0, 0, 1, 0.1 ); - glVertexPointer( 2, GL_FLOAT, 0, pts ); glDrawArrays( GL_POLYGON, 0, sample_count+1 ); PopColor(); } if( showLaserStrikes ) { - // draw the beam strike points in black - PushColor( 0, 0, 0, 1.0 ); - glPointSize( 1.0 ); + // draw the beam strike points + PushColor( 0, 0, 1, 0.8 ); glDrawArrays( GL_POINTS, 0, sample_count+1 ); PopColor(); } Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/model_load.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -138,21 +138,15 @@ blockgroup.CalcSize(); double epsilon = 0.01; - double width = blockgroup.size.x; - double height = blockgroup.size.y; - double dx = width/2.0; - double dy = height/2.0; - double dz = blockgroup.size.z; + stg_size_t bgsize = blockgroup.GetSize(); - // add thin bounding blocks - AddBlockRect(-dx,-dy, epsilon, height, dz ); - AddBlockRect(-dx,-dy, width, epsilon, dz ); - AddBlockRect(-dx, -dy+height-epsilon, width, epsilon, dz ); - AddBlockRect(-dx+width-epsilon,-dy, epsilon, height, dz ); + AddBlockRect(blockgroup.minx,blockgroup.miny, epsilon, bgsize.y, bgsize.z ); + AddBlockRect(blockgroup.minx,blockgroup.miny, bgsize.x, epsilon, bgsize.z ); + AddBlockRect(blockgroup.minx,blockgroup.maxy-epsilon, bgsize.x, epsilon, bgsize.z ); + AddBlockRect(blockgroup.maxx-epsilon,blockgroup.miny, epsilon, bgsize.y, bgsize.z ); } } - if( wf->PropertyExists( wf_entity, "mass" )) this->SetMass( wf->ReadFloat(wf_entity, "mass", this->mass )); @@ -206,7 +200,7 @@ this->Say( wf->ReadString(wf_entity, "say", NULL )); // call any type-specific load callbacks - this->CallCallbacks( &this->load_hook ); + this->CallCallbacks( &this->hook_load ); // MUST BE THE LAST THING LOADED if( wf->PropertyExists( wf_entity, "alwayson" )) @@ -261,7 +255,7 @@ } // call any type-specific save callbacks - this->CallCallbacks( &this->save_hook ); + this->CallCallbacks( &this->hook_save ); PRINT_DEBUG1( "Model \"%s\" saving complete.", token ); } Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/region.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -16,7 +16,7 @@ Region::Region() : count(0) { - for( int i=0; i<Region::SIZE; i++ ) + for( unsigned int i=0; i<Region::SIZE; i++ ) cells[i].region = this; } @@ -33,7 +33,7 @@ origin.y = y; // initialize the parent pointer for all my child regions - for( int i=0; i<SuperRegion::SIZE; i++ ) + for( unsigned int i=0; i<SuperRegion::SIZE; i++ ) regions[i].superregion = this; } Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/stage.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -233,6 +233,7 @@ return( pixels + index ); } +/* static void pb_set_pixel( Fl_Shared_Image* pb, int x, int y, uint8_t val ) { // bounds checking @@ -249,6 +250,7 @@ else PRINT_WARN4( "pb_set_pixel coordinate %d,%d out of range (image dimensions %d by %d)", x, y, width, height ); } +*/ // set all the pixels in a rectangle static void pb_set_rect( Fl_Shared_Image* pb, int x, int y, int width, int height, uint8_t val ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/stage.hh 2008-12-05 22:31:14 UTC (rev 7189) @@ -288,6 +288,7 @@ stg_pose_t() : x(0.0), y(0.0), z(0.0), a(0.0) { /*empty*/ } + virtual ~stg_pose_t(){}; static stg_pose_t Random( stg_meters_t xmin, stg_meters_t xmax, stg_meters_t ymin, stg_meters_t ymax ) @@ -300,7 +301,7 @@ virtual void Print( const char* prefix ) { - printf( "%d pose [x:%.3f y:%.3f a:%.3f]\n", + printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x,y,z,a ); } }; @@ -324,7 +325,7 @@ virtual void Print( const char* prefix ) { - printf( "%d velocity [x:%.3f y:%.3f a:%.3f]\n", + printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", prefix, x,y,z,a ); } }; @@ -337,7 +338,7 @@ stg_pose_t pose; //< position stg_size_t size; //< extent - virtual void Print( const char* prefix ) + void Print( const char* prefix ) { printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n", prefix, @@ -360,14 +361,18 @@ stg_color_t color; }; - /** bound a range of values, from min to max */ - typedef struct + /** bound a range of values, from min to max. min and max are initialized to zero. */ + class stg_bounds_t { + public: double min; //< smallest value in range double max; //< largest value in range - } stg_bounds_t; - - /** bound a volume along the x,y,z axes */ + + stg_bounds_t() : min(0), max(0) + { /* empty*/ }; + }; + + /** bound a volume along the x,y,z axes. All bounds initialized to zero. */ typedef struct { stg_bounds_t x; //< volume extent along x axis @@ -375,14 +380,8 @@ stg_bounds_t z; //< volume extent along z axis } stg_bounds3d_t; - /** bound a range of range values, from min to max */ - typedef struct - { - stg_meters_t min; //< smallest value in range - stg_meters_t max; //< largest value in range - } stg_range_bounds_t; - /** define a three-dimensional bounding box */ + /** define a three-dimensional bounding box, initialized to zero */ typedef struct { stg_bounds_t x, y, z; @@ -394,7 +393,7 @@ stg_bounds_t range; //< min and max range of sensor stg_radians_t angle; //< width of viewing angle of sensor } stg_fov_t; - + /** define a point on a 2d plane */ typedef struct { @@ -881,21 +880,44 @@ static GList* world_list; ///< all the worlds that exist static bool quit_all; ///< quit all worlds ASAP + static void UpdateCb( StgWorld* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - stg_usec_t real_time_start; ///< the real time at which this world was created bool destroy; - bool quit; ///< quit this world ASAP - stg_usec_t real_time_now; ///< The current real time in microseconds bool dirty; ///< iff true, a gui redraw would be required - int total_subs; ///< the total number of subscriptions to all models - double ppm; ///< the resolution of the world model in pixels per meter GHashTable* models_by_name; ///< the models that make up the world, indexed by name - + double ppm; ///< the resolution of the world model in pixels per meter + bool quit; ///< quit this world ASAP /** StgWorld::quit is set true when this simulation time is reached */ stg_usec_t quit_time; + stg_usec_t real_time_now; ///< The current real time in microseconds + stg_usec_t real_time_start; ///< the real time at which this world was created + GMutex* thread_mutex; + GThreadPool *threadpool; + int total_subs; ///< the total number of subscriptions to all models + unsigned int update_jobs_pending; + GList* velocity_list; ///< Models with non-zero velocity and should have their poses updated + unsigned int worker_threads; + GCond* worker_threads_done; - // hint that the world needs to be redrawn if a GUI is attached + protected: + + stg_bounds3d_t extent; ///< Describes the 3D volume of the world + bool graphics;///< true iff we have a GUI + stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps + GList* ray_list;///< List of rays traced for debug visualization + stg_usec_t sim_time; ///< the current sim time in this world in ms + GHashTable* 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 + long unsigned int updates; ///< the number of simulated time steps executed so far + Worldfile* wf; ///< If set, points to the worldfile used to create this world + + public: + 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 + + /** hint that the world needs to be redrawn if a GUI is attached */ void NeedRedraw(){ dirty = true; }; void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); @@ -958,22 +980,6 @@ const uint32_t sample_count, const bool ztest ); - protected: - - static void UpdateCb( StgWorld* world); - - GHashTable* superregions; - stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps - stg_usec_t sim_time; ///< the current sim time in this world in ms - GList* ray_list;///< List of rays traced for debug visualization - Worldfile* wf; ///< If set, points to the worldfile used to create this world - bool graphics;///< true iff we have a GUI - stg_bounds3d_t extent; ///< Describes the 3D volume of the world - long unsigned int updates; ///< the number of simulated time steps executed so far - FileManager fileMan; ///< Used to load and save worldfiles - SuperRegion* sr_cached; ///< The last superregion looked up by this world - GList* velocity_list; ///< Models with non-zero velocity and should have their poses updated - GList* update_list; ///< Models that have a subscriber or controller, and need to be updated /** Enlarge the bounding volume to include this point */ void Extend( stg_point3_t pt ); @@ -1011,54 +1017,44 @@ velocity_list = g_list_remove( velocity_list, mod ); } - static void update_thread_entry( StgModel* mod, void* count ); + static void update_thread_entry( StgModel* mod, StgWorld* world ); - GMutex* thread_mutex; - GCond* worker_threads_done; - GThreadPool *threadpool; - unsigned int worker_threads; - public: - 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 - static bool UpdateAll(); //returns true when time to quit, false otherwise - + /** returns true when time to quit, false otherwise */ + static bool UpdateAll(); + StgWorld( const char* token = "MyWorld", stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM, double ppm = DEFAULT_PPM ); - + virtual ~StgWorld(); stg_usec_t SimTimeNow(void){ return sim_time; } stg_usec_t RealTimeNow(void); stg_usec_t RealTimeSinceStart(void); - + stg_usec_t GetSimInterval(){ return interval_sim; }; - + Worldfile* GetWorldFile(){ return wf; }; - + inline virtual bool IsGUI() { return false; } - + virtual void Load( const char* worldfile_path ); virtual void UnLoad(); virtual void Reload(); virtual bool Save( const char* filename ); virtual bool Update(void); - + bool TestQuit(){ return( quit || quit_all ); } void Quit(){ quit = true; } void QuitAll(){ quit_all = true; } void CancelQuit(){ quit = false; } void CancelQuitAll(){ quit_all = false; } - + /** Get the resolution in pixels-per-metre of the underlying discrete raytracing model */ double Resolution(){ return ppm; }; - - /** Returns a pointer to the model identified by ID, or NULL if - nonexistent */ - //StgModel* GetModel( const stg_id_t id ); - + /** Returns a pointer to the model identified by name, or NULL if nonexistent */ StgModel* GetModel( const char* name ); @@ -1068,11 +1064,6 @@ /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() { return updates; } - - -// stg_point_t* LocalToGlobal( double scalex, double scaley, -// stg_point_t pts[], -// uint32_t pt_count ); }; class StgBlock @@ -1130,16 +1121,18 @@ StgModel* TestCollision(); void SwitchToTestedCells(); - - void Load( Worldfile* wf, int entity ); - - StgModel* mod; //< model to which this block belongs - - stg_color_t GetColor(); - - private: - stg_point_t* pts; //< points defining a polygon + + void Load( Worldfile* wf, int entity ); + + StgModel* GetModel(){ return mod; }; + + stg_color_t GetColor(); + +private: + StgModel* mod; //< model to which this block belongs + size_t pt_count; //< the number of points + stg_point_t* pts; //< points defining a polygon stg_size_t size; @@ -1173,19 +1166,26 @@ class BlockGroup { - + friend class StgModel; + private: int displaylist; void BuildDisplayList( StgModel* mod ); - public: GList* blocks; uint32_t count; stg_size_t size; stg_point3_t offset; + stg_meters_t minx, maxx, miny, maxy; + public: BlockGroup(); ~BlockGroup(); + + GList* GetBlocks(){ return blocks; }; + uint32_t GetCount(){ return count; }; + stg_size_t GetSize(){ return size; }; + stg_point3_t GetOffset(){ return offset; }; /** establish the min and max of all the blocks, so we can scale this group later */ @@ -1231,85 +1231,87 @@ /** the number of models instatiated - used to assign unique IDs */ static uint32_t count; static GHashTable* modelsbyid; - /** unique process-wide identifier for this model */ - uint32_t id; std::vector<Option*> drawOptions; const std::vector<Option*>& getOptions() const { return drawOptions; } protected: - static const char* typestr; + //static const char* typestr; - stg_pose_t pose; - stg_velocity_t velocity; - stg_watts_t watts; //< power consumed by this model - stg_color_t color; - stg_kg_t mass; + GMutex* access_mutex; + GPtrArray* blinkenlights; + bool blob_return; + BlockGroup blockgroup; + /** OpenGL display list identifier for the blockgroup */ + int blocks_dl; + + int boundary; + + /** callback functions can be attached to any field in this + structure. When the internal function model_change(<mod>,<field>) + is called, the callback is triggered */ + GHashTable* callbacks; + + stg_color_t color; + /** This can be set to indicate that the model has new data that + may be of interest to users. This allows polling the model + instead of adding a data callback. */ + bool data_fresh; + stg_bool_t disabled; //< if non-zero, the model is disabled + int fiducial_key; + int fiducial_return; + GList* flag_list; stg_geom_t geom; - stg_laser_return_t laser_return; - int obstacle_return; - int blob_return; - int gripper_return; - int ranger_return; - int fiducial_return; - int fiducial_key; - int boundary; - stg_meters_t map_resolution; - stg_bool_t stall; - StgModel* parent; //< the model that owns this one, possibly NUL - GArray* trail; stg_pose_t global_pose; - bool rebuild_displaylist; //< iff true, regenerate block display list before redraw bool gpose_dirty; //< set this to indicate that global pose may have changed - bool map_caches_are_invalid; - int subs; //< the number of subscriptions to this model - bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() - stg_usec_t interval; //< time between updates in us - stg_usec_t last_update; //< time of last update in us - stg_bool_t disabled; //< if non-zero, the model is disabled - char* say_string; //< if non-null, this string is displayed in the GUI - StgWorld* world; // pointer to the world in which this model exists - ctrlinit_t* initfunc; - GList* flag_list; - Worldfile* wf; - int wf_entity; - GPtrArray* blinkenlights; - int blocks_dl; //< OpenGL display list identifier - stg_model_type_t type; - BlockGroup blockgroup; - bool has_default_block; - bool on_velocity_list; - bool on_update_list; - + bool gripper_return; + int gui_grid; + int gui_mask; int gui_nose; - int gui_grid; int gui_outline; - int gui_mask; - + bool has_default_block; /* hooks for attaching special callback functions (not used as variables) */ - char startup_hook, shutdown_hook, load_hook, save_hook, update_hook; - + char hook_load; + char hook_save; + char hook_shutdown; + char hook_startup; + char hook_update; + /** unique process-wide identifier for this model */ + uint32_t id; + ctrlinit_t* initfunc; + stg_usec_t interval; //< time between updates in us + stg_laser_return_t laser_return; + stg_usec_t last_update; //< time of last update in us + bool map_caches_are_invalid; + stg_meters_t map_resolution; + stg_kg_t mass; + bool obstacle_return; + bool on_update_list; + bool on_velocity_list; + StgModel* parent; //< the model that owns this one, possibly NUL /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code to associate arbitrary items with a model. */ + stg_pose_t pose; GData* props; + bool ranger_return; + bool rebuild_displaylist; //< iff true, regenerate block display list before redraw + char* say_string; //< if non-null, this string is displayed in the GUI - /** callback functions can be attached to any field in this - structure. When the internal function model_change(<mod>,<field>) - is called, the callback is triggered */ - GHashTable* callbacks; - - bool data_fresh; ///< this can be set to indicate that the model has - ///new data that may be of interest to users. This - ///allows polling the model instead of adding a - ///data callback. - + stg_bool_t stall; /** Thread safety flag. Iff true, Update() may be called in parallel with other models. Defaults to false for safety */ + int subs; //< the number of subscriptions to this model bool thread_safe; - - GMutex* access_mutex; - + GArray* trail; + stg_model_type_t type; + bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() + stg_velocity_t velocity; + stg_watts_t watts; //< power consumed by this model + Worldfile* wf; + int wf_entity; + StgWorld* world; // pointer to the world in which this model exists + public: void Lock() { @@ -1657,34 +1659,34 @@ some implementation detail */ void AddStartupCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &startup_hook, cb, user ); }; + { AddCallback( &hook_startup, cb, user ); }; void RemoveStartupCallback( stg_model_callback_t cb ) - { RemoveCallback( &startup_hook, cb ); }; + { RemoveCallback( &hook_startup, cb ); }; void AddShutdownCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &shutdown_hook, cb, user ); }; + { AddCallback( &hook_shutdown, cb, user ); }; void RemoveShutdownCallback( stg_model_callback_t cb ) - { RemoveCallback( &shutdown_hook, cb ); } + { RemoveCallback( &hook_shutdown, cb ); } void AddLoadCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &load_hook, cb, user ); } + { AddCallback( &hook_load, cb, user ); } void RemoveLoadCallback( stg_model_callback_t cb ) - { RemoveCallback( &load_hook, cb ); } + { RemoveCallback( &hook_load, cb ); } void AddSaveCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &save_hook, cb, user ); } + { AddCallback( &hook_save, cb, user ); } void RemoveSaveCallback( stg_model_callback_t cb ) - { RemoveCallback( &save_hook, cb ); } + { RemoveCallback( &hook_save, cb ); } void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &update_hook, cb, user ); } + { AddCallback( &hook_update, cb, user ); } void RemoveUpdateCallback( stg_model_callback_t cb ) - { RemoveCallback( &update_hook, cb ); } + { RemoveCallback( &hook_update, cb ); } /** named-property interface */ @@ -1892,17 +1894,19 @@ friend class StgModelCamera; private: - bool paused; ///< the world only updates when this is false - //int wf_section; + StgCanvas* canvas; + std::vector<Option*> drawOptions; + FileManager fileMan; ///< Used to load and save worldfiles + stg_usec_t interval_log[INTERVAL_LOG_LEN]; + stg_usec_t interval_real; ///< real-time interval between updates - set this to zero for 'as fast as possible Fl_Menu_Bar* mbar; OptionsDlg* oDlg; - std::vector<Option*> drawOptions; - void updateOptions(); - stg_usec_t interval_log[INTERVAL_LOG_LEN]; + bool pause_time; + bool paused; ///< the world only updates when this is false + stg_usec_t real_time_of_last_update; - stg_usec_t real_time_of_last_update; - stg_usec_t interval_real; ///< real-time interval between updates - set this to zero for 'as fast as possible + void UpdateOptions(); // static callback functions static void windowCb( Fl_Widget* w, void* p ); @@ -1917,10 +1921,7 @@ // GUI functions bool saveAsDialog(); bool closeWindowQuery(); - - // Quit time pause - bool pause_time; - + virtual void AddModel( StgModel* mod ); protected: @@ -1934,7 +1935,6 @@ StgCanvas* GetCanvas( void ) { return canvas; } public: - static const stg_msec_t DEFAULT_INTERVAL_REAL = 100; ///< real time between updates StgWorldGui(int W,int H,const char*L=0); ~StgWorldGui(); @@ -1952,7 +1952,10 @@ void Start(){ paused = false; }; void Stop(){ paused = true; }; void TogglePause(){ paused = !paused; }; - + + /** show the window - need to call this if you don't Load(). */ + void Show(); + /** Get human readable string that describes the current simulation time. */ std::string ClockString( void ); @@ -2081,7 +2084,7 @@ { uint32_t sample_count; uint32_t resolution; - stg_range_bounds_t range_bounds; + stg_bounds_t range_bounds; stg_radians_t fov; stg_usec_t interval; } stg_laser_cfg_t; @@ -2090,8 +2093,6 @@ class StgModelLaser : public StgModel { private: - int dl_debug_laser; - /** OpenGL displaylist for laser data */ int data_dl; bool data_dirty; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/world.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -53,10 +53,6 @@ bool StgWorld::quit_all = false; GList* StgWorld::world_list = NULL; -const unsigned int THREAD_COUNT = 8; - -gint update_jobs_pending=0; - static guint PointIntHash( stg_point_int_t* pt ) { return( pt->x + (pt->y<<16 )); @@ -68,6 +64,63 @@ } +StgWorld::StgWorld( const char* token, + stg_msec_t interval_sim, + double ppm ) + : + // private + destroy( false ), + dirty( true ), + models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), + ppm( ppm ), // raytrace resolution + quit( false ), + quit_time( 0 ), + real_time_now( RealTimeNow() ), + real_time_start( real_time_now ), + thread_mutex( g_mutex_new() ), + threadpool( NULL ), + total_subs( 0 ), + update_jobs_pending(0), + velocity_list( NULL ), + worker_threads( 0 ), + worker_threads_done( g_cond_new() ), + + // protected + extent(), + graphics( false ), + interval_sim( (stg_usec_t)thousand * interval_sim ), + ray_list( NULL ), + sim_time( 0 ), + superregions( g_hash_table_new( (GHashFunc)PointIntHash, + (GEqualFunc)PointIntEqual ) ), + sr_cached(NULL), + update_list( NULL ), + updates( 0 ), + wf( NULL ) +{ + if( ! Stg::InitDone() ) + { + PRINT_WARN( "Stg::Init() must be called before a StgWorld is created." ); + exit(-1); + } + + StgWorld::world_list = g_list_append( StgWorld::world_list, this ); +} + + +StgWorld::~StgWorld( void ) +{ + PRINT_DEBUG2( "destroying world %d %s", id, token ); + + if( wf ) delete wf; + + g_hash_table_destroy( models_by_name ); + g_free( token ); + + world_list = g_list_remove( world_list, this ); +} + + SuperRegion* StgWorld::CreateSuperRegion( int32_t x, int32_t y ) { SuperRegion* sr = new SuperRegion( x, y ); @@ -94,74 +147,21 @@ return quit; } -void StgWorld::update_thread_entry( StgModel* mod, void* dummy ) +void StgWorld::update_thread_entry( StgModel* mod, StgWorld* world ) { mod->Update(); - g_mutex_lock( mod->world->thread_mutex ); + g_mutex_lock( world->thread_mutex ); - update_jobs_pending--; + world->update_jobs_pending--; - if( update_jobs_pending == 0 ) - g_cond_signal( mod->world->worker_threads_done ); + if( world->update_jobs_pending == 0 ) + g_cond_signal( world->worker_threads_done ); - g_mutex_unlock( mod->world->thread_mutex ); + g_mutex_unlock( world->thread_mutex ); } -StgWorld::StgWorld( const char* token, - stg_msec_t interval_sim, - double ppm ) - : - ray_list( NULL ), - quit_time( 0 ), - quit( false ), - updates( 0 ), - wf( NULL ), - graphics( false ), - models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), - sim_time( 0 ), - interval_sim( (stg_usec_t)thousand * interval_sim ), - ppm( ppm ), // raytrace resolution - real_time_start( RealTimeNow() ), - superregions( g_hash_table_new( (GHashFunc)PointIntHash, - (GEqualFunc)PointIntEqual ) ), - total_subs( 0 ), - destroy( false ), - real_time_now( 0 ), - velocity_list( NULL ), - update_list( NULL ), - sr_cached(NULL), - worker_threads( 0 ), - threadpool( NULL ), - thread_mutex( g_mutex_new() ), - worker_threads_done( g_cond_new() ) -{ - if( ! Stg::InitDone() ) - { - PRINT_WARN( "Stg::Init() must be called before a StgWorld is created." ); - exit(-1); - } - - bzero( &extent, sizeof(extent) ); - - StgWorld::world_list = g_list_append( StgWorld::world_list, this ); -} - - -StgWorld::~StgWorld( void ) -{ - PRINT_DEBUG2( "destroying world %d %s", id, token ); - - if( wf ) delete wf; - - g_hash_table_destroy( models_by_name ); - g_free( token ); - - world_list = g_list_remove( world_list, this ); -} - - void StgWorld::RemoveModel( StgModel* mod ) { g_hash_table_remove( models_by_name, mod ); @@ -293,7 +293,7 @@ if( threadpool == NULL ) threadpool = g_thread_pool_new( (GFunc)update_thread_entry, - NULL, + this, worker_threads, true, NULL ); @@ -334,9 +334,12 @@ LISTMETHOD( children, StgModel*, InitRecursive ); stg_usec_t load_end_time = RealTimeNow(); - - printf( "[Load time %.3fsec]\n", - (load_end_time - load_start_time) / 1000000.0 ); + + if( debug ) + printf( "[Load time %.3fsec]\n", + (load_end_time - load_start_time) / 1000000.0 ); + else + putchar( '\n' ); } @@ -755,6 +758,7 @@ static int _save_cb( StgModel* mod, void* dummy ) { mod->Save(); + return 0; } bool StgWorld::Save( const char *filename ) @@ -767,6 +771,7 @@ static int _reload_cb( StgModel* mod, void* dummy ) { mod->Load(); + return 0; } // reload the current worldfile Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/libstage/worldgui.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -154,48 +154,51 @@ StgWorldGui::StgWorldGui(int W,int H,const char* L) : Fl_Window(W,H,L ), + canvas( new StgCanvas( this,0,30,W,H-30 ) ), + drawOptions(), + fileMan(), + interval_log(), + interval_real( (stg_usec_t)1e5 ), + mbar( new Fl_Menu_Bar(0,0, W, 30)), oDlg( NULL ), + pause_time( false ), paused( false ), - //graphics( true ), - pause_time( false ), - interval_real( (stg_usec_t)thousand * DEFAULT_INTERVAL_REAL ), - mbar( new Fl_Menu_Bar(0,0, W, 30)), - canvas( new StgCanvas( this,0,30,W,H-30 ) ) - { - for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ ) - interval_log[i] = interval_real; - - resizable(canvas); + real_time_of_last_update( RealTimeNow() ) +{ + for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ ) + interval_log[i] = interval_real; + + resizable(canvas); + + end(); + + label( PROJECT ); + + mbar->textsize(12); + + mbar->add( "&File", 0, 0, 0, FL_SUBMENU ); + mbar->add( "File/&Load World...", FL_CTRL + 'l', fileLoadCb, this, FL_MENU_DIVIDER ); + mbar->add( "File/&Save World", FL_CTRL + 's', fileSaveCb, this ); + mbar->add( "File/Save World &As...", FL_CTRL + FL_SHIFT + 's', StgWorldGui::fileSaveAsCb, this, FL_MENU_DIVIDER ); + + //mbar->add( "File/Screenshots", 0,0,0, FL_SUBMENU ); + //mbar->add( "File/Screenshots/Save Frames, fileScreenshotSaveCb, this,FL_MENU_TOGGLE ); + + mbar->add( "File/E&xit", FL_CTRL+'q', StgWorldGui::fileExitCb, this ); + + mbar->add( "&View", 0, 0, 0, FL_SUBMENU ); + mbar->add( "View/Filter data...", FL_SHIFT + 'd', StgWorldGui::viewOptionsCb, this ); + canvas->createMenuItems( mbar, "View" ); + + mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); + mbar->add( "Help/&About Stage...", 0, StgWorldGui::helpAboutCb, this ); + //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); + + callback( StgWorldGui::windowCb, this ); - end(); + show(); +} - label( PROJECT ), - - mbar->textsize(12); - - mbar->add( "&File", 0, 0, 0, FL_SUBMENU ); - mbar->add( "File/&Load World...", FL_CTRL + 'l', fileLoadCb, this, FL_MENU_DIVIDER ); - mbar->add( "File/&Save World", FL_CTRL + 's', fileSaveCb, this ); - mbar->add( "File/Save World &As...", FL_CTRL + FL_SHIFT + 's', StgWorldGui::fileSaveAsCb, this, FL_MENU_DIVIDER ); - - //mbar->add( "File/Screenshots", 0,0,0, FL_SUBMENU ); - //mbar->add( "File/Screenshots/Save Frames, fileScreenshotSaveCb, this,FL_MENU_TOGGLE ); - - mbar->add( "File/E&xit", FL_CTRL+'q', StgWorldGui::fileExitCb, this ); - - mbar->add( "&View", 0, 0, 0, FL_SUBMENU ); - mbar->add( "View/Filter data...", FL_SHIFT + 'd', StgWorldGui::viewOptionsCb, this ); - canvas->createMenuItems( mbar, "View" ); - - mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); - mbar->add( "Help/&About Stage...", 0, StgWorldGui::helpAboutCb, this ); - //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); - - callback( StgWorldGui::windowCb, this ); - - show(); - } - StgWorldGui::~StgWorldGui() { delete mbar; @@ -204,6 +207,11 @@ delete canvas; } +void StgWorldGui::Show() +{ + show(); // fltk +} + void StgWorldGui::Load( const char* filename ) { PRINT_DEBUG1( "%s.Load()", token ); @@ -245,7 +253,9 @@ } label( title.c_str() ); - updateOptions(); + UpdateOptions(); + + show(); } void StgWorldGui::UnLoad() @@ -279,9 +289,6 @@ bool StgWorldGui::Update() { - if( real_time_of_last_update == 0 ) - real_time_of_last_update = RealTimeNow(); - //pause the simulation if quit time is set if( PastQuitTime() && pause_time == false ) { TogglePause(); @@ -318,6 +325,9 @@ } } while( interval < interval_real ); + + //printf( "interval_real %.20f\n", interval_real ); + // if( paused ) // gentle on the CPU when paused //usleep( 10000 ); @@ -579,7 +589,7 @@ void StgWorldGui::helpAboutCb( Fl_Widget* w, void* p ) { - StgWorldGui* worldGui = static_cast<StgWorldGui*>( p ); + // StgWorldGui* worldGui = static_cast<StgWorldGui*>( p ); fl_register_images(); @@ -683,18 +693,21 @@ } } -void StgWorldGui::updateOptions() { +void StgWorldGui::UpdateOptions() +{ std::set<Option*, Option::optComp> options; std::vector<Option*> modOpts; - for( GList* it=update_list; it; it=it->next ) { - modOpts = ((StgModel*)it->data)->getOptions(); - options.insert( modOpts.begin(), modOpts.end() ); - } + + for( GList* it=update_list; it; it=it->next ) + { + modOpts = ((StgModel*)it->data)->getOptions(); + options.insert( modOpts.begin(), modOpts.end() ); + } drawOptions.assign( options.begin(), options.end() ); - if ( oDlg ) { - oDlg->setOptions( drawOptions ); - } + + if ( oDlg ) + oDlg->setOptions( drawOptions ); } void StgWorldGui::DrawBoundingBoxTree() Modified: code/stage/trunk/worlds/benchmark/expand.cc =================================================================== --- code/stage/trunk/worlds/benchmark/expand.cc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/worlds/benchmark/expand.cc 2008-12-05 22:31:14 UTC (rev 7189) @@ -84,7 +84,6 @@ //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed ); - int forward = 0 ; // if the front is clear, drive forwards if( (rgr->samples[0] > SAFE_DIST) && (rgr->samples[1] > SAFE_DIST/2.0) && Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/worlds/fasr.world 2008-12-05 22:31:14 UTC (rev 7189) @@ -13,8 +13,9 @@ resolution 0.02 -# threads don't help here as the per-robot update is very fast -threadpool 0 +# threads may help here depending on your CPU +threadpool 3 +# threadpool 2 # configure the GUI window Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/worlds/simple.world 2008-12-05 22:31:14 UTC (rev 7189) @@ -5,7 +5,6 @@ include "pioneer.inc" include "map.inc" include "sick.inc" -include "walle.inc" interval_sim 100 # simulation timestep in milliseconds interval_real 10 # real-time interval between simulation updates in milliseconds @@ -29,9 +28,9 @@ floorplan ( name "cave" - size [17.000 17.000 0.800] - pose [8.000 8.000 0 0 ] - bitmap "bitmaps/cave.png" + size [16.000 16.000 0.800] + pose [8 8 0 0] + bitmap "bitmaps/cave.png" ) @@ -40,10 +39,8 @@ # can refer to the robot by this name name "r0" - localization "odom" - pose [ 0.892 0.800 0 56.500 ] - sicklaser( samples 180 ) + sicklaser() ctrl "wander" ) Modified: code/stage/trunk/worlds/walle.inc =================================================================== --- code/stage/trunk/worlds/walle.inc 2008-12-04 21:30:45 UTC (rev 7188) +++ code/stage/trunk/worlds/walle.inc 2008-12-05 22:31:14 UTC (rev 7189) @@ -21,8 +21,6 @@ size [0.35 0.5 0.5] #origin [-0.25 -0.25 0 0 ] - laser( pose [0.15 0 -0.27 0 ] size [0.01 0.01 0.01] alwayson 1 ) - # body bottom block ( This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |