diff --git a/.gitignore b/.gitignore index b7e85fee5..a9a99e996 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build config.h +*.swp diff --git a/AUTHORS.txt b/AUTHORS.txt index d72e7c60c..dd605e8be 100644 --- a/AUTHORS.txt +++ b/AUTHORS.txt @@ -14,7 +14,7 @@ terms of the GNU General Public License version 2 or later: - Jeremy Asher (jra11@sfu.ca) - Alex Couture-Beil (asc17@sfu.ca) - Geoff Biggs (git@killbots.net) - +- Rich Mattes (jpgr87@github.com) Many patches and bug reports have been contributed by users around the world. Stage is part of the Player Project (http://playerstage.org), diff --git a/CMakeLists.txt b/CMakeLists.txt index 1392116e4..2230f33c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,7 @@ MESSAGE (STATUS "${PROJECT_NAME} version ${VERSION}") SET (PROJECT_CMAKE_DIR ${PROJECT_SOURCE_DIR}/cmake CACHE PATH "Location of CMake scripts") # Determine the operating system in detail INCLUDE (${PROJECT_CMAKE_DIR}/internal/FindOS.cmake) - +INCLUDE (${PROJECT_CMAKE_DIR}/internal/UninstallTarget.cmake) # Enable -Wall by default unless on Win or Solaris IF (NOT PROJECT_OS_WIN AND NOT PROJECT_OS_SOLARIS) # Using -Wall on Windows causes MSVC to produce thousands of warnings in its @@ -40,9 +40,9 @@ ENDIF (NOT PROJECT_OS_WIN AND NOT PROJECT_OS_SOLARIS) ##################################### # Build type cflags -SET (CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG ${WALL} " CACHE INTERNAL "C Flags for release" FORCE) +SET (CMAKE_CXX_FLAGS_RELEASE " -DNDEBUG ${WALL} " CACHE INTERNAL "C Flags for release" FORCE) SET (CMAKE_CXX_FLAGS_DEBUG " -ggdb ${WALL} " CACHE INTERNAL "C Flags for debug" FORCE) -SET (CMAKE_CXX_FLAGS_PROFILE " -O3 -ggdb -pg ${WALL} " CACHE INTERNAL "C Flags for profile" FORCE) +SET (CMAKE_CXX_FLAGS_PROFILE " -ggdb -pg ${WALL} " CACHE INTERNAL "C Flags for profile" FORCE) ##################################### # Set the default build type @@ -140,6 +140,16 @@ STRING(REGEX REPLACE "\n" ";" FLTK_LIBS "${FLTK_LIBS_NEWLINES}") #MESSAGE( STATUS " FLTK_LDFLAGS = ${FLTK_LDFLAGS}") #MESSAGE( STATUS " FLTK_LIBS = ${FLTK_LIBS}") +STRING(REGEX REPLACE " -lGLU" "" TMP "${FLTK_LDFLAGS}") +STRING(REGEX REPLACE " -lGL" "" TMP2 "${TMP}") +STRING(REGEX REPLACE " -lpng" "" FLTK_LDFLAGS_PKGCONFIG "${TMP2}") + + +STRING(REGEX REPLACE " -lgl" "" FLTK_LIBS_PKGCONFIG "${FLTK_LIBS}") +STRING(REGEX REPLACE " -lglu" "" FLTK_LIBS_PKGCONFIG" "${FLTK_LIBS_PKGCONFIG}") +STRING(REGEX REPLACE " -lpng" "" FLTK_LIBS_PKGCONFIG" "${FLTK_LIBS_PKGCONFIG}") + + SET (FLTK_FOUND TRUE) # ENDIF( BUILD_GUI ) @@ -162,7 +172,7 @@ IF( PLAYER_FOUND ) # MESSAGE( STATUS " PLAYER_CFLAGS: ${PLAYER_CFLAGS}" ) # MESSAGE( STATUS " PLAYER_LDFLAGS: ${PLAYER_LDFLAGS}" ) ELSE( PLAYER_FOUND ) - MESSAGE( ${INDENT} "Player not detected. If Player is installed but not detected, check your PKG_CONFIG_PATH." ) + MESSAGE(STATUS ${INDENT} "Player not detected. If Player is installed but not detected, check your PKG_CONFIG_PATH." ) ENDIF( PLAYER_FOUND ) @@ -178,7 +188,7 @@ ENDIF( PLAYER_FOUND ) # Create the pkgconfig file CONFIGURE_FILE (${CMAKE_CURRENT_SOURCE_DIR}/stage.pc.in ${CMAKE_CURRENT_BINARY_DIR}/stage.pc @ONLY) -INSTALL (FILES ${CMAKE_CURRENT_BINARY_DIR}/stage.pc DESTINATION lib/pkgconfig/) +INSTALL (FILES ${CMAKE_CURRENT_BINARY_DIR}/stage.pc DESTINATION ${PROJECT_LIB_DIR}/pkgconfig/) MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}" ) diff --git a/cmake/internal/FindOS.cmake b/cmake/internal/FindOS.cmake index aebbd2b98..cec449124 100644 --- a/cmake/internal/FindOS.cmake +++ b/cmake/internal/FindOS.cmake @@ -43,3 +43,30 @@ ELSEIF (PROJECT_OS_SOLARIS) ELSE (PROJECT_OS_LINUX) MESSAGE (STATUS "Operating system is generic Unix") ENDIF (PROJECT_OS_LINUX) + +# 32 or 64 bit Linux +IF (PROJECT_OS_LINUX) + # Set the library directory suffix accordingly + IF (${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64") + SET (PROJECT_PROC_64BIT TRUE BOOL INTERNAL) + MESSAGE (STATUS "Linux x86_64 Detected") + ELSEIF (${CMAKE_SYSTEM_PROCESSOR} STREQUAL "ppc64") + MESSAGE (STATUS "Linux ppc64 Detected") + SET (PROJECT_PROC_64BIT TRUE BOOL INTERNAL) + ELSEIF ($CMAKE_SYSTEM_PROCESSOR} STREQUAL "s390x") + MESSAGE (STATEUS "Linux s390x Detected") + SET (PROJECT_PROC_64BIT TRUE BOOL INTERNAL) + ENDIF (${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64") +ENDIF (PROJECT_OS_LINUX) + +IF(PROJECT_PROC_64BIT) + # Set the install path to lib64 + SET(PROJECT_LIB_DIR "lib64") + SET(PROJECT_PLUGIN_DIR "lib64/${PROJECT_NAME}-${APIVERSION}") +ELSE (PROJECT_PROC_64BIT) + SET(PROJECT_LIB_DIR "lib") + SET(PROJECT_PLUGIN_DIR "lib/${PROJECT_NAME}-${APIVERSION}") +ENDIF (PROJECT_PROC_64BIT) + +MESSAGE(STATUS "Installing Libraries to ${CMAKE_INSTALL_PREFIX}/${PROJECT_LIB_DIR}") +MESSAGE(STATUS "Installing Plugins to ${CMAKE_INSTALL_PREFIX}/${PROJECT_PLUGIN_DIR}") diff --git a/cmake/internal/UninstallTarget.cmake b/cmake/internal/UninstallTarget.cmake new file mode 100644 index 000000000..6caa9c3d2 --- /dev/null +++ b/cmake/internal/UninstallTarget.cmake @@ -0,0 +1,5 @@ +# Add an "uninstall" target + +CONFIGURE_FILE ("${PROJECT_CMAKE_DIR}/internal/cmake_uninstall.cmake.in" "${PROJECT_BINARY_DIR}/cmake_uninstall.cmake" IMMEDIATE @ONLY) + +ADD_CUSTOM_TARGET (uninstall "${CMAKE_COMMAND}" -P "${PROJECT_BINARY_DIR}/cmake_uninstall.cmake") diff --git a/cmake/internal/cmake_uninstall.cmake.in b/cmake/internal/cmake_uninstall.cmake.in new file mode 100644 index 000000000..df95fb9d8 --- /dev/null +++ b/cmake/internal/cmake_uninstall.cmake.in @@ -0,0 +1,21 @@ +IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + MESSAGE(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"") +ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") + +FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) +STRING(REGEX REPLACE "\n" ";" files "${files}") +FOREACH(file ${files}) + MESSAGE(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") + IF(EXISTS "$ENV{DESTDIR}${file}") + EXEC_PROGRAM( + "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" + OUTPUT_VARIABLE rm_out + RETURN_VALUE rm_retval + ) + IF(NOT "${rm_retval}" STREQUAL 0) + MESSAGE(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") + ENDIF(NOT "${rm_retval}" STREQUAL 0) + ELSE(EXISTS "$ENV{DESTDIR}${file}") + MESSAGE(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") + ENDIF(EXISTS "$ENV{DESTDIR}${file}") +ENDFOREACH(file) diff --git a/config.h.in b/config.h.in index d753eeaee..ae08c6078 100644 --- a/config.h.in +++ b/config.h.in @@ -5,6 +5,7 @@ #define VERSION "@VERSION@" #define APIVERSION "@APIVERSION@" #define INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@" +#define PLUGIN_PATH "@CMAKE_INSTALL_PREFIX@/@PROJECT_PLUGIN_DIR@" #cmakedefine BUILD_GUI diff --git a/docsrc/Markdown.pl b/docsrc/Markdown.pl old mode 100755 new mode 100644 diff --git a/docsrc/sourcedocs.sh b/docsrc/sourcedocs.sh old mode 100755 new mode 100644 diff --git a/docsrc/upload.sh b/docsrc/upload.sh old mode 100755 new mode 100644 diff --git a/examples/ctrl/CMakeLists.txt b/examples/ctrl/CMakeLists.txt index 3fdb3498f..0a27868a5 100644 --- a/examples/ctrl/CMakeLists.txt +++ b/examples/ctrl/CMakeLists.txt @@ -23,30 +23,19 @@ foreach( PLUGIN ${PLUGINS} ) endforeach( PLUGIN ) ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp ) -#ADD_LIBRARY( fasr3 MODULE fasr3.cc astar/findpath.cpp ) # add fasr2 to the list of plugins -SET( PLUGINS ${PLUGINS} fasr2)# fasr3) +SET( PLUGINS ${PLUGINS} fasr2 ) set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) foreach( PLUGIN ${PLUGINS} ) - TARGET_LINK_LIBRARIES( ${PLUGIN} stage ) + TARGET_LINK_LIBRARIES( ${PLUGIN} stage ${OPENGL_LIBRARIES} ) endforeach( PLUGIN ) - # delete the "lib" prefix from the plugin libraries SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" ) -# need plaer's wavefront planning library for this one -#if( PLAYER_FOUND ) -# link_directories( ${PLAYER_LIBRARY_DIRS} ) -# include_directories( ${PLAYER_INCLUDE_DIRS} ) -# target_link_libraries( fasr_plan "-lwavefront_standalone" ) -#endif( PLAYER_FOUND ) - - - # install in /lib -install( TARGETS ${PLUGINS} fasr2 DESTINATION lib) #fasr3 +install( TARGETS ${PLUGINS} DESTINATION ${PROJECT_PLUGIN_DIR}) diff --git a/examples/ctrl/astar/findpath.cpp b/examples/ctrl/astar/findpath.cpp old mode 100755 new mode 100644 diff --git a/examples/ctrl/astar/fsa.h b/examples/ctrl/astar/fsa.h old mode 100755 new mode 100644 diff --git a/examples/ctrl/astar/license.txt b/examples/ctrl/astar/license.txt old mode 100755 new mode 100644 diff --git a/examples/ctrl/astar/readme.txt b/examples/ctrl/astar/readme.txt old mode 100755 new mode 100644 diff --git a/examples/ctrl/astar/stlastar.h b/examples/ctrl/astar/stlastar.h old mode 100755 new mode 100644 diff --git a/examples/libplayerc/playerswarm/Makefile b/examples/libplayerc/playerswarm/Makefile old mode 100755 new mode 100644 diff --git a/examples/libplayerc/playerswarm/args.h b/examples/libplayerc/playerswarm/args.h old mode 100755 new mode 100644 diff --git a/examples/libplayerc/playerswarm/many.sh b/examples/libplayerc/playerswarm/many.sh old mode 100755 new mode 100644 diff --git a/libstage/#blockgroup.cc# b/libstage/#blockgroup.cc# new file mode 100644 index 000000000..53cab864b --- /dev/null +++ b/libstage/#blockgroup.cc# @@ -0,0 +1,333 @@ + +#include "stage.hh" +#include "worldfile.hh" + +#include // for dirname(3) +#include // for _POSIX_PATH_MAX + +#undef DEBUG + +using namespace Stg; +using namespace std; + +BlockGroup::BlockGroup() + : displaylist(0), + blocks(), + minx(0), + maxx(0), + miny(0), + maxy(0) +{ /* empty */ } + +BlockGroup::~BlockGroup() +{ + Clear(); +} + +void BlockGroup::AppendBlock( Block* block ) +{ + blocks.insert( block ); +} + +void BlockGroup::Clear() +{ + FOR_EACH( it, blocks ) + delete *it; + + blocks.clear(); +} + +void BlockGroup::SwitchToTestedCells() +{ + // confirm the tentative pose for all blocks + FOR_EACH( it, blocks ) + (*it)->SwitchToTestedCells(); +} + +void BlockGroup::AppendTouchingModels( ModelPtrSet &v ) +{ + FOR_EACH( it, blocks ) + (*it)->AppendTouchingModels( v ); +} + +Model* BlockGroup::TestCollision() +{ + Model* hitmod = NULL; + + FOR_EACH( it, blocks ) + if( (hitmod = (*it)->TestCollision())) + break; // bail on the earliest collision + + return hitmod; // NULL if no collision +} + + +// establish the min and max of all the blocks, so we can scale this +// group later +void BlockGroup::CalcSize() +{ + // assuming the blocks currently fit in a square +/- one billion units + //double minx, miny, maxx, maxy, minz, maxz; + minx = miny = billion; + maxx = maxy = -billion; + + size.z = 0.0; // grow to largest z we see + + FOR_EACH( it, blocks ) + { + // examine all the points in the polygon + Block* block = *it; + + FOR_EACH( it, block->pts ) + { + if( it->x < minx ) minx = it->x; + if( it->y < miny ) miny = it->y; + if( it->x > maxx ) maxx = it->x; + if( it->y > maxy ) maxy = it->y; + } + + size.z = std::max( block->local_z.max, size.z ); + } + + // 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? + + InvalidateModelPointCache(); +} + + +void BlockGroup::Map() +{ + FOR_EACH( it, blocks ) + (*it)->Map(); +} + +void BlockGroup::UnMap() +{ + FOR_EACH( it, blocks ) + (*it)->UnMap(); +} + +void BlockGroup::DrawSolid( const Geom & 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 ); + + FOR_EACH( it, blocks ) + (*it)->DrawSolid(); + + glPopMatrix(); +} + +void BlockGroup::DrawFootPrint( const Geom & 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 ); + + FOR_EACH( it, blocks ) + (*it)->DrawFootPrint(); + + glPopMatrix(); +} + +void BlockGroup::BuildDisplayList( Model* mod ) +{ + //puts( "build" ); + + if( ! mod->world->IsGUI() ) + return; + + //printf( "display list for model %s\n", mod->token ); + if( displaylist == 0 ) + { + displaylist = glGenLists(1); + CalcSize(); + } + + glNewList( displaylist, GL_COMPILE ); + + Geom 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); + + mod->PushColor( mod->color ); + + FOR_EACH( it, blocks ) + { + Block* blk = (*it); + + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + mod->PushColor( blk->color ); + blk->DrawSolid(); + mod->PopColor(); + } + else + blk->DrawSolid(); + } + + mod->PopColor(); + + glDisable(GL_POLYGON_OFFSET_FILL); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glDepthMask(GL_FALSE); + + Color c = mod->color; + c.r /= 2.0; + c.g /= 2.0; + c.b /= 2.0; + mod->PushColor( c ); + + //c.Print( "color" ); + + + FOR_EACH( it, blocks ) + { + Block* blk = *it; + + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + Color c = blk->color; + c.r /= 2.0; + c.g /= 2.0; + c.b /= 2.0; + mod->PushColor( c ); + //c.Print( "bar" ); + blk->DrawSolid(); + mod->PopColor(); + } + else + blk->DrawSolid(); + } + + glDepthMask(GL_TRUE); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + mod->PopColor(); + + glEndList(); +} + +void BlockGroup::CallDisplayList( Model* mod ) +{ + if( displaylist == 0 || mod->rebuild_displaylist ) + { + BuildDisplayList( mod ); + mod->rebuild_displaylist = 0; + } + + glCallList( displaylist ); +} + +void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity ) +{ + AppendBlock( new Block( mod, wf, entity )); +} + +void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile* wf ) +{ + Print_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile ); + + char full[_POSIX_PATH_MAX]; + + if( bitmapfile[0] == '/' ) + strncpy( full, bitmapfile.c_str(), _POSIX_PATH_MAX ); + else + { + char *tmp = strdup(wf->filename.c_str()); + snprintf( full, _POSIX_PATH_MAX, + "%s/%s", dirname(tmp), bitmapfile.c_str() ); + free(tmp); + } + + PRINT_DEBUG1( "attempting to load image %s", full ); + + stg_rotrect_t* rects = NULL; + unsigned int rect_count = 0; + unsigned int width, height; + if( stg_rotrects_from_image_file( full, + &rects, + &rect_count, + &width, &height ) ) + { + PRINT_ERR1( "failed to load rects from image file \"%s\"", + full ); + return; + } + + //printf( "found %d rects in \"%s\" at %p\n", + // rect_count, full, rects ); + + if( rects && (rect_count > 0) ) + { + // TODO fix this + Color col( 1.0, 0.0, 1.0, 1.0 ); + + for( unsigned int r=0; rRasterize( data, width, height, cellwidth, cellheight ); +} diff --git a/libstage/CMakeLists.txt b/libstage/CMakeLists.txt index f28e0e3ea..6649cc442 100644 --- a/libstage/CMakeLists.txt +++ b/libstage/CMakeLists.txt @@ -48,10 +48,10 @@ set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAG add_library(stage SHARED ${stageSrcs}) -target_link_libraries( stage - ${OPENGL_LIBRARIES} - ${LTDL_LIB} - ${FLTK_LIBS2} +target_link_libraries( stage + ${LTDL_LIB} +# ${OPENGL_LIBRARIES} # is it safe to remove these permanently? +# ${FLTK_LIBS2} ) # causes the shared library to have a version number @@ -61,19 +61,25 @@ set_target_properties( stage PROPERTIES ) set( stagebinarySrcs main.cc ) +set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) 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 ) +# Apple seems to have trouble when libstage and stagebinary are linked against FLTK +# Newer Linux distributions won't allow stagebinary to inherit libstage's links to fltk, so we need +# to explicitly link on Linux -target_link_libraries( stagebinary stage ) +if (NOT PROJECT_OS_LINUX) + set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage ) + target_link_libraries( stagebinary stage ) +ELSE (NOT PROJECT_OS_LINUX) + set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage LINK_FLAGS "${FLTK_LDFLAGS}" ) + target_link_libraries( stagebinary stage pthread ) +ENDIF(NOT PROJECT_OS_LINUX) INSTALL(TARGETS stagebinary stage RUNTIME DESTINATION bin - LIBRARY DESTINATION lib + LIBRARY DESTINATION ${PROJECT_LIB_DIR} ) INSTALL(FILES stage.hh diff --git a/libstage/block.cc b/libstage/block.cc index 8f6423ea7..4baea3531 100644 --- a/libstage/block.cc +++ b/libstage/block.cc @@ -11,12 +11,13 @@ static void canonicalize_winding(vector& pts); blocks. The point data is copied, so pts can safely be freed after calling this.*/ Block::Block( Model* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - Color color, - bool inherit_color ) : + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + Color color, + bool inherit_color, + bool wheel ) : mod( mod ), mpts(), pt_count( pt_count ), @@ -24,6 +25,7 @@ Block::Block( Model* mod, local_z( zmin, zmax ), color( color ), inherit_color( inherit_color ), + wheel(wheel), rendered_cells( new CellPtrVec ), candidate_cells( new CellPtrVec ), gpts() @@ -462,8 +464,28 @@ void Block::DrawFootPrint() void Block::DrawSolid() { - DrawSides(); - DrawTop(); +// if( wheel ) +// { +// glPushMatrix(); + +// glRotatef( 90,0,1,0 ); +// glRotatef( 90,1,0,0 ); + +// glTranslatef( -local_z.max /2.0, 0, 0 ); + + +// GLUquadric* quadric = gluNewQuadric(); +// gluQuadricDrawStyle( quadric, GLU_FILL ); +// gluCylinder( quadric, local_z.max, local_z.max, size.x, 16, 16 ); +// gluDeleteQuadric( quadric ); + +// glPopMatrix(); +// } +// else + { + DrawSides(); + DrawTop(); + } } void Block::Load( Worldfile* wf, int entity ) @@ -483,8 +505,8 @@ void Block::Load( Worldfile* wf, int entity ) local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 ); local_z.max = wf->ReadTupleLength( entity, "z", 1, 1.0 ); - const std::string& colorstr = wf->ReadString( entity, "color", "" ); - + const std::string& colorstr = wf->ReadString( entity, "color", "" ); + if( colorstr != "" ) { color = Color( colorstr ); @@ -492,6 +514,8 @@ void Block::Load( Worldfile* wf, int entity ) } else inherit_color = true; + + wheel = wf->ReadInt( entity, "wheel", wheel ); } ///////////////////////////////////////////////////////////////////////////////////////// diff --git a/libstage/blockgroup.cc b/libstage/blockgroup.cc index 18efe8bc4..bcf03f0b8 100644 --- a/libstage/blockgroup.cc +++ b/libstage/blockgroup.cc @@ -310,10 +310,11 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil pts[3].y = y + h; AppendBlock( new Block( mod, - pts,4, - 0,1, - col, - true ) ); + pts,4, + 0,1, + col, + true, + false) ); } free( rects ); } diff --git a/libstage/color.cc b/libstage/color.cc index 6278c4f20..f3c57e917 100644 --- a/libstage/color.cc +++ b/libstage/color.cc @@ -40,6 +40,7 @@ Color::Color( const std::string& name) : PRINT_ERR1("unable to open color database: %s " "(try adding rgb.txt's location to your STAGEPATH)", strerror(errno)); + exit(0); } diff --git a/libstage/main.cc b/libstage/main.cc index c3610ebf9..1ed37de14 100644 --- a/libstage/main.cc +++ b/libstage/main.cc @@ -95,12 +95,13 @@ int main( int argc, char* argv[] ) } optindex++; } - + if( usegui ) - Fl::run(); + Fl::run(); else while( ! World::UpdateAll() ); puts( "\n[Stage: done]" ); - exit(0); + + return EXIT_SUCCESS; } diff --git a/libstage/model.cc b/libstage/model.cc index 1f6187d57..1bc28c7f5 100644 --- a/libstage/model.cc +++ b/libstage/model.cc @@ -24,6 +24,8 @@ origin [ 0.0 0.0 0.0 0.0 ] velocity [ 0.0 0.0 0.0 0.0 ] + update_interval 100 + color "red" color_rgba [ 0.0 0.0 0.0 1.0 ] bitmap "" @@ -70,6 +72,10 @@ that if the model hits an obstacle, its velocity will be set to zero. + - update_interval int (defaults to 100) The amount of simulated + time in milliseconds between calls to Model::Update(). Controls + the frequency with which this model's data is generated. + - velocity_enable int (defaults to 0)\n Most models ignore their velocity state. This saves on processing time since most models never have their velocity set to anything but zero. Some @@ -342,18 +348,20 @@ Model::Model( World* world, Model::~Model( void ) { - UnMap(); // remove from the raytrace bitmap - // children are removed in ancestor class // remove myself from my parent's child list, or the world's child // list if I have no parent - ModelPtrVec& vec = parent ? parent->children : world->children; - EraseAll( this, vec ); - modelsbyid.erase(id); + if( world ) // if I'm not a worldless dummy model + { + UnMap(); // remove from the raytrace bitmap - world->RemoveModel( this ); + ModelPtrVec& vec = parent ? parent->children : world->children; + EraseAll( this, vec ); + modelsbyid.erase(id); + world->RemoveModel( this ); + } } @@ -445,7 +453,8 @@ Block* Model::AddBlockRect( stg_meters_t x, pts, 4, 0, dz, color, - true ); + true, + false ); blockgroup.AppendBlock( newblock ); @@ -453,7 +462,7 @@ Block* Model::AddBlockRect( stg_meters_t x, } -stg_raytrace_result_t Model::Raytrace( const Pose &pose, +RaytraceResult Model::Raytrace( const Pose &pose, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, @@ -467,7 +476,7 @@ stg_raytrace_result_t Model::Raytrace( const Pose &pose, ztest ); } -stg_raytrace_result_t Model::Raytrace( const stg_radians_t bearing, +RaytraceResult Model::Raytrace( const stg_radians_t bearing, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, @@ -491,7 +500,7 @@ void Model::Raytrace( const stg_radians_t bearing, const stg_radians_t fov, const stg_ray_test_func_t func, const void* arg, - stg_raytrace_result_t* samples, + RaytraceResult* samples, const uint32_t sample_count, const bool ztest ) { diff --git a/libstage/model_blobfinder.cc b/libstage/model_blobfinder.cc index 5a9222f90..85bae8bc7 100644 --- a/libstage/model_blobfinder.cc +++ b/libstage/model_blobfinder.cc @@ -180,7 +180,7 @@ void ModelBlobfinder::Update( void ) { // generate a scan for post-processing into a blob image - stg_raytrace_result_t* samples = new stg_raytrace_result_t[scan_width]; + RaytraceResult* samples = new RaytraceResult[scan_width]; Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false ); diff --git a/libstage/model_draw.cc b/libstage/model_draw.cc index 569d1194e..5e058cc24 100644 --- a/libstage/model_draw.cc +++ b/libstage/model_draw.cc @@ -15,11 +15,11 @@ void Model::DrawSelected() glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid - Pose gpose = GetGlobalPose(); + Pose gp = GetGlobalPose(); char buf[64]; snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]", - token.c_str(), gpose.x, gpose.y, gpose.z, rtod(gpose.a) ); + token.c_str(), gp.x, gp.y, gp.z, rtod(gp.a) ); PushColor( 0,0,0,1 ); // text color black Gl::draw_string( 0.5,0.5,0.5, buf ); @@ -44,7 +44,82 @@ void Model::DrawSelected() glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PopColor(); + + // testing + + // highlight all fiducial robots within a certain range + +// Gl::pose_inverse_shift( gp ); + +// double rng = 10.0; + +// Model left( gp.x - rng, gp.y, world ); +// Model right( gp.x + rng, gp.y, world ); +// Model down( gp.x, gp.y - rng, world ); +// Model up( gp.x, gp.y + rng, world ); + +// std::set::iterator xmin = world->models_with_fiducials_byx.lower_bound( &left ); +// std::set::iterator xmax = world->models_with_fiducials_byx.upper_bound( &right ); + +// std::set::iterator ymin = world->models_with_fiducials_byy.lower_bound( &down ); +// std::set::iterator ymax = world->models_with_fiducials_byy.upper_bound( &up ); + + +// PushColor( Color(1,0,0,0.5) ); + + +// std::vector candidates; +// std::set horiz, vert; + +// while( xmin != xmax ) +// { +// //candidates.insert( *xmin ); +// horiz.insert( *xmin); + +// Pose op = (*xmin)->GetGlobalPose(); +// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 ); + +// xmin++; +// } + +// PopColor(); + +// PushColor( Color(0,0,1,0.5) ); + +// while( ymin != ymax ) +// { +// vert.insert( *ymin ); + +// // candidates.insert( *ymin ); + +// Pose op = (*ymin)->GetGlobalPose(); +// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 ); +// ymin++; +// } + +// PopColor(); + +// PushColor( Color(0,1,0,0.5) ); + +// std::set_intersection( horiz.begin(), horiz.end(), +// vert.begin(), vert.end(), +// std::inserter( candidates, candidates.end() ) ); + +// //printf( "cand sz %lu\n", candidates.size() ); + +// glTranslatef( 0,0,1.0 ); + +// FOR_EACH( it, candidates ) +// { +// Pose op = (*it)->GetGlobalPose(); +// glRectf( op.x - 5, op.y - 5, op.x + 5, op.y + 5 ); +// } + +// PopColor(); + + glPopMatrix(); + } diff --git a/libstage/model_fiducial.cc b/libstage/model_fiducial.cc index e84410f7b..3f55fd6e2 100644 --- a/libstage/model_fiducial.cc +++ b/libstage/model_fiducial.cc @@ -176,7 +176,7 @@ void ModelFiducial::AddModelIfVisible( Model* him ) //printf( "range %.2f\n", range ); - stg_raytrace_result_t ray( Raytrace( dtheta, + RaytraceResult ray( Raytrace( dtheta, max_range_anon, // TODOscan only as far as the object fiducial_raytrace_match, NULL, @@ -209,6 +209,8 @@ void ModelFiducial::AddModelIfVisible( Model* him ) fid.geom.y = hisgeom.size.y; fid.geom.a = normalize( hispose.a - mypose.a); + //fid.pose_rel = hispose - this->GetGlobalPose(); + // store the global pose of the fiducial (mainly for the GUI) fid.pose = hispose; @@ -222,6 +224,7 @@ void ModelFiducial::AddModelIfVisible( Model* him ) fiducials.push_back( fid ); } + /////////////////////////////////////////////////////////////////////////// // Update the beacon data // @@ -231,12 +234,64 @@ void ModelFiducial::Update( void ) if( subs < 1 ) return; - + // reset the array of detected fiducials fiducials.clear(); + +#if( 1 ) + // BEGIN EXPERIMENT + + // find two sets of fiducial-bearing models, within sensor range on + // the two different axes + + double rng = max_range_anon; + Pose gp = GetGlobalPose(); + Model edge; // dummy model used to find bounds in the sets + + edge.pose = Pose( gp.x-rng, gp.y, 0, 0 ); // LEFT + std::set::iterator xmin = + world->models_with_fiducials_byx.lower_bound( &edge ); // O(log(n)) + + edge.pose = Pose( gp.x+rng, gp.y, 0, 0 ); // RIGHT + const std::set::iterator xmax = + world->models_with_fiducials_byx.upper_bound( &edge ); + + edge.pose = Pose( gp.x, gp.y-rng, 0, 0 ); // BOTTOM + std::set::iterator ymin = + world->models_with_fiducials_byy.lower_bound( &edge ); + + edge.pose = Pose( gp.x, gp.y+rng, 0, 0 ); // TOP + const std::set::iterator ymax = + world->models_with_fiducials_byy.upper_bound( &edge ); + + // put these models into sets keyed on model pointer, rather than position + std::set horiz, vert; + + for( ; xmin != xmax; xmin++) + horiz.insert( *xmin); + + for( ; ymin != ymax; ymin++ ) + vert.insert( *ymin ); + + // the intersection of the sets is all the fiducials close by + std::vector nearby; + std::set_intersection( horiz.begin(), horiz.end(), + vert.begin(), vert.end(), + std::inserter( nearby, nearby.end() ) ); - FOR_EACH( it, world->models_with_fiducials ) - AddModelIfVisible( *it ); + // printf( "cand sz %lu\n", nearby.size() ); + + // create sets sorted by x and y position + FOR_EACH( it, nearby ) + AddModelIfVisible( *it ); +#else + // create sets sorted by x and y position + FOR_EACH( it, world->models_with_fiducials ) + AddModelIfVisible( *it ); + +#endif + + // find the range of fiducials within range in X Model::Update(); } diff --git a/libstage/model_getset.cc b/libstage/model_getset.cc index ae78a99bd..262826cb5 100644 --- a/libstage/model_getset.cc +++ b/libstage/model_getset.cc @@ -211,11 +211,11 @@ void Model::SetPose( const Pose& newpose ) // if( isnan( pose.a ) ) // printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", // token, pose.x, pose.y, pose.z, pose.a ); - + NeedRedraw(); MapWithChildren(); world->dirty = true; } - - CallCallbacks( CB_POSE ); + + CallCallbacks( CB_POSE ); } diff --git a/libstage/model_gripper.cc b/libstage/model_gripper.cc index 7548f7fa0..b14370048 100644 --- a/libstage/model_gripper.cc +++ b/libstage/model_gripper.cc @@ -359,7 +359,7 @@ void ModelGripper::UpdateBreakBeams() double bbr = (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 )); - stg_raytrace_result_t sample = + RaytraceResult sample = Raytrace( pz, // ray origin bbr, // range gripper_raytrace_match, // match func @@ -410,7 +410,7 @@ void ModelGripper::UpdateContacts() // paddle beam max range double bbr = cfg.paddle_size.x * geom.size.x; - stg_raytrace_result_t leftsample = + RaytraceResult leftsample = Raytrace( lpz, // ray origin bbr, // range gripper_raytrace_match, // match func @@ -419,7 +419,7 @@ void ModelGripper::UpdateContacts() cfg.contact[0] = leftsample.mod; - stg_raytrace_result_t rightsample = + RaytraceResult rightsample = Raytrace( rpz, // ray origin bbr, // range gripper_raytrace_match, // match func diff --git a/libstage/model_laser.cc b/libstage/model_laser.cc index 754bfcfbd..1cffc2879 100644 --- a/libstage/model_laser.cc +++ b/libstage/model_laser.cc @@ -186,18 +186,20 @@ void ModelLaser::Update( void ) // trace the ray, incrementing its heading for each sample for( unsigned int t(0); tRaytrace( ray ) ); + RaytraceResult r( world->Raytrace( ray ) ); samples[t].range = r.range; // if we hit a model and it reflects brightly, we set // reflectance high, else low if( r.mod && ( r.mod->vis.laser_return >= LaserBright ) ) - samples[t].reflectance = 1; + samples[t].reflectance = 1.0; else - samples[t].reflectance = 0; + samples[t].reflectance = 0.0; // point the ray to the next angle ray.origin.a += sample_incr; + + } // we may need to interpolate the samples we skipped @@ -259,6 +261,11 @@ const std::vector& ModelLaser::GetSamples() const return samples; } +std::vector& ModelLaser::GetSamples() +{ + return samples; +} + // VIS ------------------------------------------------------------------- diff --git a/libstage/model_load.cc b/libstage/model_load.cc index a001d3099..bc943fcf9 100644 --- a/libstage/model_load.cc +++ b/libstage/model_load.cc @@ -10,6 +10,7 @@ #include "stage.hh" #include "worldfile.hh" #include "file_manager.hh" +#include "config.h" using namespace Stg; //#define DEBUG @@ -227,8 +228,9 @@ void Model::Load() LoadControllerModule( lib ); } } - - + + // internally interval is in usec, but we use msec in worldfiles + interval = 1000 * wf->ReadInt( wf_entity, "update_interval", interval/1000 ); Say( wf->ReadString(wf_entity, "say", "" )); @@ -324,6 +326,9 @@ void Model::LoadControllerModule( const char* lib ) lt_dlsetsearchpath( FileManager::stagePath().c_str() ); + // PLUGIN_PATH now defined in config.h + lt_dladdsearchdir( PLUGIN_PATH ); + lt_dlhandle handle = NULL; // the library name is the first word in the string diff --git a/libstage/model_ranger.cc b/libstage/model_ranger.cc index 92da3eb29..b58ef8035 100644 --- a/libstage/model_ranger.cc +++ b/libstage/model_ranger.cc @@ -265,7 +265,7 @@ void ModelRanger::Update( void ) //for( int r=0; rdata()[0]); diff --git a/libstage/stage.hh b/libstage/stage.hh index a3f1b88f6..1e32801ca 100644 --- a/libstage/stage.hh +++ b/libstage/stage.hh @@ -226,6 +226,9 @@ namespace Stg bool operator==( const Color& other ) const; static Color RandomColor(); void Print( const char* prefix ) const; + + /** convenient constants */ + static const Color blue, red, green, yellow, magenta, cyan; }; /** specify a rectangular size */ @@ -742,8 +745,6 @@ namespace Stg stg_meters_t range ) : pose(pose), range(range), mod(NULL), color() {} }; - - typedef RaytraceResult stg_raytrace_result_t; class Ray { @@ -754,12 +755,12 @@ namespace Stg Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true) {} - - const Model* mod; - Pose origin; - stg_meters_t range; - stg_ray_test_func_t func; - const void* arg; + + const Model* mod; + Pose origin; + stg_meters_t range; + stg_ray_test_func_t func; + const void* arg; bool ztest; }; @@ -840,7 +841,20 @@ namespace Stg /** Keep a list of all models with detectable fiducials. This avoids searching the whole world for fiducials. */ ModelPtrVec models_with_fiducials; - + + struct ltx + { + bool operator()(const Model* a, const Model* b) const; + }; + + struct lty + { + bool operator()(const Model* a, const Model* b) const; + }; + + std::set models_with_fiducials_byx; + std::set models_with_fiducials_byy; + /** Add a model to the set of models with non-zero fiducials, if not already there. */ void FiducialInsert( Model* mod ) { @@ -967,9 +981,9 @@ namespace Stg void DestroySuperRegion( SuperRegion* sr ); /** trace a ray. */ - stg_raytrace_result_t Raytrace( const Ray& ray ); + RaytraceResult Raytrace( const Ray& ray ); - stg_raytrace_result_t Raytrace( const Pose& pose, + RaytraceResult Raytrace( const Pose& pose, const stg_meters_t range, const stg_ray_test_func_t func, const Model* finder, @@ -982,7 +996,7 @@ namespace Stg const stg_ray_test_func_t func, const Model* finder, const void* arg, - stg_raytrace_result_t* samples, + RaytraceResult* samples, const uint32_t sample_count, const bool ztest ); @@ -1123,6 +1137,9 @@ namespace Stg /** Returns a pointer to the model identified by name, or NULL if nonexistent */ Model* GetModel( const std::string& name ) const; + + /** Returns a const reference to the set of models in the world. */ + const std::set GetAllModels() const { return models; }; /** Return the 3D bounding box of the world, in meters */ const stg_bounds3d_t& GetExtent() const { return extent; }; @@ -1154,12 +1171,13 @@ namespace Stg blocks. The point data is copied, so pts can safely be freed after constructing the block.*/ Block( Model* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - Color color, - bool inherit_color ); + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + Color color, + bool inherit_color, + bool wheel ); /** A from-file constructor */ Block( Model* mod, Worldfile* wf, int entity); @@ -1223,7 +1241,8 @@ namespace Stg Bounds local_z; ///< z extent in local coords Color color; bool inherit_color; - + bool wheel; + void DrawTop(); void DrawSides(); @@ -1691,7 +1710,8 @@ namespace Stg friend class BlockGroup; friend class PowerPack; friend class Ray; - + friend class ModelFiducial; + private: /** the number of models instatiated - used to assign unique IDs */ static uint32_t count; @@ -2029,7 +2049,7 @@ namespace Stg /** raytraces a single ray from the point and heading identified by pose, in local coords */ - stg_raytrace_result_t Raytrace( const Pose &pose, + RaytraceResult Raytrace( const Pose &pose, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, @@ -2042,11 +2062,11 @@ namespace Stg const stg_radians_t fov, const stg_ray_test_func_t func, const void* arg, - stg_raytrace_result_t* samples, + RaytraceResult* samples, const uint32_t sample_count, const bool ztest = true ); - stg_raytrace_result_t Raytrace( const stg_radians_t bearing, + RaytraceResult Raytrace( const stg_radians_t bearing, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, @@ -2057,7 +2077,7 @@ namespace Stg const stg_radians_t fov, const stg_ray_test_func_t func, const void* arg, - stg_raytrace_result_t* samples, + RaytraceResult* samples, const uint32_t sample_count, const bool ztest = true ); @@ -2134,7 +2154,12 @@ namespace Stg /** Destructor */ virtual ~Model(); - + + /** Alternate constructor that created dummy models with only a pose */ + Model() + : parent(NULL), world(NULL) + {} + void Say( const std::string& str ); /** Attach a user supplied visualization to a model. */ @@ -2385,6 +2410,15 @@ namespace Stg bool HasSubscribers() const { return( subs > 0 ); } static std::map< std::string, creator_t> name_map; + +// class Neighbors +// { +// Model *left, *right, *up, *down; +// public: +// Neighbors() : left(NULL), right(NULL), up(NULL), down(NULL) {} +// } nbors; // instance + + }; @@ -2446,6 +2480,8 @@ namespace Stg return &blobs[0]; } + std::vector GetBlobs() const { return blobs; } + /** Start finding blobs with this color.*/ void AddColor( Color col ); @@ -2471,7 +2507,7 @@ namespace Stg { public: stg_meters_t range; ///< range to laser hit in meters - double reflectance; ///< intensity of the reflection 0.0 to 1.0 + double reflectance; ///< intensity of the reflection 0.0 to 1.0 }; /** Convenience object for setting parameters using SetConfig/GetConfig */ @@ -2502,6 +2538,7 @@ namespace Stg unsigned int sample_count; std::vector samples; + public: stg_meters_t range_max; stg_radians_t fov; uint32_t resolution; @@ -2527,6 +2564,9 @@ namespace Stg /** returns a const reference to a vector of range and reflectance samples */ const std::vector& GetSamples() const; + /** returns a mutable reference to a vector of range and reflectance samples */ + std::vector& GetSamples(); + /** Get the user-tweakable configuration of the laser */ Config GetConfig( ) const; @@ -2678,6 +2718,7 @@ namespace Stg stg_meters_t range; ///< range to the target stg_radians_t bearing; ///< bearing to the target Pose geom; ///< size and relative angle of the target + Pose pose_rel; /// relative pose of the target in local coordinates Pose pose; ///< Absolute accurate position of the target in world coordinates (it's cheating to use this in robot controllers!) Model* mod; ///< Pointer to the model (real fiducial detectors can't do this!) int id; ///< the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected. @@ -2857,7 +2898,7 @@ namespace Stg ///get a reference to camera depth buffer const GLfloat* FrameDepth() const { return _frame_data; } - ///get a reference to camera color image. 3 bytes (RGB) per pixel + ///get a reference to camera color image. 4 bytes (RGBA) per pixel const GLubyte* FrameColor() const { return _frame_color_data; } ///change the pitch @@ -3032,6 +3073,8 @@ namespace Stg double GetMaxPosition() const {return max_position;}; double GetMinPosition() const {return min_position;}; + ActuatorType GetType() const { return actuator_type; } + stg_point3_t GetAxis() const { return axis; } }; diff --git a/libstage/world.cc b/libstage/world.cc index 65fb38639..0e769a39e 100644 --- a/libstage/world.cc +++ b/libstage/world.cc @@ -96,6 +96,30 @@ #include "option.hh" using namespace Stg; +// // function objects for comparing model positions +bool World::ltx::operator()(const Model* a, const Model* b) const +{ + stg_meters_t ax = a->GetGlobalPose().x; + stg_meters_t bx = b->GetGlobalPose().x; + + if( ax == bx ) + return a < b; // tie breaker + + return ax < bx; +} +bool World::lty::operator()(const Model* a, const Model* b) const +{ + stg_meters_t ay = a->GetGlobalPose().y; + stg_meters_t by = b->GetGlobalPose().y; + + if( ay == by ) + return a < b; // tie breaker + + return ay < by; +} + + + // static data members unsigned int World::next_id = 0; bool World::quit_all = false; @@ -112,6 +136,8 @@ World::World( const std::string& name, models(), models_by_name(), models_with_fiducials(), + models_with_fiducials_byx(), + models_with_fiducials_byy(), ppm( ppm ), // raytrace resolution quit( false ), show_clock( false ), @@ -186,12 +212,18 @@ bool World::UpdateAll() { bool quit = true; + puts( "updateall" ); + FOR_EACH( world_it, World::world_set ) { + puts( "world" ); + if( (*world_it)->Update() == false ) quit = false; } + printf( "quit = %d\n", quit ); + return quit; } @@ -446,7 +478,7 @@ void World::UnLoad() bool World::PastQuitTime() { - return( (quit_time > 0) && (sim_time >= quit_time) ); + return( (quit_time > 0) && (sim_time >= quit_time) ); } std::string World::ClockString() const @@ -536,7 +568,6 @@ void World::ConsumeQueue( unsigned int queue_num ) } } - bool World::Update() { if( show_clock && ((this->updates % show_clock_interval) == 0) ) @@ -556,6 +587,22 @@ bool World::Update() FOR_EACH( it, active_velocity ) (*it)->UpdatePose(); + + + // rebuild the sets sorted by position on x,y axis +#if( 1 ) + models_with_fiducials_byx.clear(); + models_with_fiducials_byy.clear(); + + FOR_EACH( it, models_with_fiducials ) + { + models_with_fiducials_byx.insert( *it ); + models_with_fiducials_byy.insert( *it ); + } +#endif + + //printf( "x %lu y %lu\n", models_with_fiducials_byy.size(), + // models_with_fiducials_byx.size() ); // handle the zeroth queue synchronously in the main thread ConsumeQueue( 0 ); @@ -649,7 +696,7 @@ void World::Raytrace( const Pose &gpose, // global pose const stg_ray_test_func_t func, const Model* model, const void* arg, - stg_raytrace_result_t* samples, // preallocated storage for samples + RaytraceResult* samples, // preallocated storage for samples const uint32_t sample_count, // number of samples const bool ztest ) { @@ -665,7 +712,7 @@ void World::Raytrace( const Pose &gpose, // global pose } // Stage spends 50-99% of its time in this method. -stg_raytrace_result_t World::Raytrace( const Pose &gpose, +RaytraceResult World::Raytrace( const Pose &gpose, const stg_meters_t range, const stg_ray_test_func_t func, const Model* mod, @@ -676,7 +723,7 @@ stg_raytrace_result_t World::Raytrace( const Pose &gpose, return Raytrace( r ); } -stg_raytrace_result_t World::Raytrace( const Ray& r ) +RaytraceResult World::Raytrace( const Ray& r ) { //rt_cells.clear(); //rt_candidate_cells.clear(); diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc index 75a5947e0..1309e985b 100644 --- a/libstage/worldgui.cc +++ b/libstage/worldgui.cc @@ -70,7 +70,7 @@ location of the center of the window in world coordinates (meters) - rotate [ ]\n angle relative to straight up, angle of rotation (degrees) - scale \n -ratio of world to pixel coordinates (window zoom) + ratio of world to pixel coordinates (window zoom) - pcam_loc [ ]\n location of the perspective camera (meters) - pcam_angle [ ]\n @@ -437,6 +437,8 @@ void WorldGui::DrawVoxels() void WorldGui::windowCb( Fl_Widget* w, WorldGui* wg ) { + puts( "callback" ); + switch ( Fl::event() ) { case FL_SHORTCUT: if ( Fl::event_key() == FL_Escape ) @@ -660,7 +662,7 @@ void WorldGui::optionsDlgCb( OptionsDlg* oDlg, WorldGui* wg ) break; //return // otherwise, ESC pressed-> do as below case FL_CLOSE: // clicked close button - // override event to close + // override event to close event = OptionsDlg::CLOSE; break; } diff --git a/libstageplugin/CMakeLists.txt b/libstageplugin/CMakeLists.txt index 943372110..b1701c119 100644 --- a/libstageplugin/CMakeLists.txt +++ b/libstageplugin/CMakeLists.txt @@ -40,6 +40,6 @@ IF (BUILD_LSPTEST) ENDIF (BUILD_LSPTEST) INSTALL(TARGETS stageplugin - LIBRARY DESTINATION lib + LIBRARY DESTINATION ${PROJECT_LIB_DIR} ) diff --git a/libstageplugin/p_simulation.cc b/libstageplugin/p_simulation.cc index f034cdbd6..3e127e7de 100644 --- a/libstageplugin/p_simulation.cc +++ b/libstageplugin/p_simulation.cc @@ -316,8 +316,6 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, } } - /* start of get/set property modifications */ - // see line 2661 of player_interfaces.h for header names and stuff // Is it a request to set a model's property? else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, @@ -342,7 +340,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, /* check the value given is an array of four floats */ if(req->value_count != sizeof(float)*4) { - PRINT_WARN("value given by SetProperty must be an array of 4 floats\n"); + PRINT_WARN("Value given by SetProperty must be an array of 4 floats\n"); return(-1); } @@ -380,6 +378,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, (player_simulation_property_req_t*)data; + // check they want to set the colour. //strncmp returns 0 if the strings match @@ -475,6 +474,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, PRINT_WARN1( "GET_PROPERTY request: simulation model \"%s\" not found", req->name ); return(-1); } + } else { @@ -484,8 +484,6 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue, } - /*end of get/set property modifications*/ - else { // Don't know how to handle this message. diff --git a/stage.pc.in b/stage.pc.in index 08e4d2434..c6f995aab 100644 --- a/stage.pc.in +++ b/stage.pc.in @@ -1,8 +1,14 @@ prefix=@CMAKE_INSTALL_PREFIX@ Name: stage -Description: Stage robot simulation program, C++ library and Player plugin - part of the Player/Stage Project +Description: Stage robot simulation program, C++ library and Player plugin - part of the Player Project (http://playerstage.org) + Version: @VERSION@ -Requires: -Libs: -L${prefix}/lib -lstage @FLTK_LDFLAGS@ + +# please don't add FLTK dependencies here, e.g. libpng, GLU, since +# these vary by platform. Let FLTK figure this out for itself and thus +# add the incantations to the flags and libs lines below +Requires: + +Libs: -L${prefix}/@PROJECT_LIB_DIR@ -lstage @FLTK_LDFLAGS_PKGCONFIG@ Cflags: -I${prefix}/include/Stage-@APIVERSION@ @FLTK_CFLAGS@ diff --git a/tests/worldfile/test.py b/tests/worldfile/test.py old mode 100755 new mode 100644 diff --git a/worlds/CMakeLists.txt b/worlds/CMakeLists.txt index 0e5b80ff2..47b75e48b 100644 --- a/worlds/CMakeLists.txt +++ b/worlds/CMakeLists.txt @@ -1,9 +1,21 @@ ADD_SUBDIRECTORY(benchmark) + +FILE( GLOB cfgs "*.cfg" ) +FILE( GLOB worlds "*.world" ) +FILE( GLOB incs "*.inc" ) +FILE( GLOB scripts "*.sh" ) + +INSTALL(FILES ${cfgs} ${worlds} ${incs} ${scripts} +DESTINATION share/stage/worlds) + +#INSTALL (DIRECTORY bitmaps wifi DESTINATION share/stage/worlds) + ADD_SUBDIRECTORY(bitmaps) +ADD_SUBDIRECTORY(wifi) -FILE(GLOB worlds "*.world") -FILE(GLOB incs "*.inc") -INSTALL(FILES ${worlds} ${incs} - DESTINATION share/stage/worlds -) +#FILE(GLOB worlds "*.world") +#FILE(GLOB incs "*.inc") +#INSTALL(FILES ${worlds} ${incs} +# DESTINATION share/stage/worlds +#) diff --git a/worlds/benchmark/CMakeLists.txt b/worlds/benchmark/CMakeLists.txt index e121668d0..398000553 100644 --- a/worlds/benchmark/CMakeLists.txt +++ b/worlds/benchmark/CMakeLists.txt @@ -10,4 +10,4 @@ TARGET_LINK_LIBRARIES( expand_pioneer stage ) set_source_files_properties( ${expand_pioneerSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) SET_TARGET_PROPERTIES( expand_pioneer PROPERTIES PREFIX "" ) -INSTALL( TARGETS expand_swarm expand_pioneer DESTINATION lib) +INSTALL( TARGETS expand_swarm expand_pioneer DESTINATION ${PROJECT_PLUGIN_DIR}) diff --git a/worlds/cfggen.sh b/worlds/cfggen.sh old mode 100755 new mode 100644 diff --git a/worlds/fasr.world b/worlds/fasr.world index ded7c5b1d..ebefbf761 100644 --- a/worlds/fasr.world +++ b/worlds/fasr.world @@ -23,7 +23,7 @@ window ( size [ 683.000 713.000 ] - center [ -0.210 -0.945 ] + center [ 0.207 -1.094 ] rotate [ 0 0 ] scale 33.591 @@ -44,7 +44,7 @@ floorplan zone ( - size [ 2 2 0 ] + size [ 2.000 2.000 0 ] color "green" pose [ -7.000 -7.000 0 0 ] name "source" @@ -53,7 +53,7 @@ zone zone ( - size [ 2 2 0 ] + size [ 2.000 2.000 0 ] color "red" pose [ 7.000 7.000 0 0 ] name "sink" @@ -96,7 +96,7 @@ define charge_station model charge_station( pose [ 7.908 -2.510 0 0 ] ) charge_station( pose [ 7.920 -3.847 0 0 ] ) charge_station( pose [ 7.449 -4.974 0 -40.107 ] ) -charge_station( pose [ 7.698 -1.432 0 31.513 ] ) +charge_station( pose [ 8.680 -1.432 0 31.513 ] ) charge_station( pose [ 5.162 -0.814 0 88.809 ] ) charge_station( pose [ 3.927 -0.693 0 88.808 ] ) @@ -107,7 +107,7 @@ charge_station( pose [ 6.427 -0.964 0 74.484 ] ) define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 64 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" kjoules 400 joules_capacity 400000 diff --git a/worlds/fasr2_open.world b/worlds/fasr2_open.world index 9d2184ea1..36b9e3a0c 100644 --- a/worlds/fasr2_open.world +++ b/worlds/fasr2_open.world @@ -21,11 +21,11 @@ threads 8 # configure the GUI window window ( - size [ 1070.000 712.000 ] + size [ 936.000 767.000 ] - center [ -28.543 -37.183 ] + center [ -22.306 -34.136 ] rotate [ 0 0 ] - scale 24.235 + scale 16.257 show_data 0 show_flags 1 @@ -48,7 +48,7 @@ define smallzone zone ) smallzone( color "red" - pose [ -32.00 -33.000 0 0 ] + pose [ -32.000 -33.000 0 0 ] name "red_source" ctrl "source 60 1" ) @@ -67,7 +67,7 @@ smallzone ) smallzone( color "DarkGreen" - pose [ 0.000 -39.000 0 0 ] + pose [ 0 -39.000 0 0 ] name "green_sink" ctrl "sink" ) diff --git a/worlds/simple.world b/worlds/simple.world index 2083a1995..9af0f3096 100644 --- a/worlds/simple.world +++ b/worlds/simple.world @@ -9,7 +9,7 @@ include "sick.inc" # time to pause (in GUI mode) or quit (in headless mode (-g)) the simulation quit_time 3600 # 1 hour of simulated time -paused 1 +paused 0 resolution 0.02 diff --git a/worlds/test.sh b/worlds/test.sh old mode 100755 new mode 100644 diff --git a/worlds/worldgen.sh b/worlds/worldgen.sh old mode 100755 new mode 100644