Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Merge remote branch 'upstream/master'

Conflicts:
	libstage/CMakeLists.txt
  • Loading branch information...
commit 6da3a7b7887ad55114081e850a254e431bbf9f22 2 parents d89e3f4 + 51d6bb8
Rich Mattes authored
Showing with 682 additions and 94 deletions.
  1. 0  docsrc/Markdown.pl
  2. 0  docsrc/sourcedocs.sh
  3. 0  docsrc/upload.sh
  4. +2 −12 examples/ctrl/CMakeLists.txt
  5. 0  examples/ctrl/astar/findpath.cpp
  6. 0  examples/ctrl/astar/fsa.h
  7. 0  examples/ctrl/astar/license.txt
  8. 0  examples/ctrl/astar/readme.txt
  9. 0  examples/ctrl/astar/stlastar.h
  10. 0  examples/libplayerc/playerswarm/Makefile
  11. 0  examples/libplayerc/playerswarm/args.h
  12. 0  examples/libplayerc/playerswarm/many.sh
  13. +333 −0 libstage/#blockgroup.cc#
  14. +2 −5 libstage/CMakeLists.txt
  15. +34 −10 libstage/block.cc
  16. +5 −4 libstage/blockgroup.cc
  17. +1 −0  libstage/color.cc
  18. +4 −3 libstage/main.cc
  19. +19 −10 libstage/model.cc
  20. +1 −1  libstage/model_blobfinder.cc
  21. +77 −2 libstage/model_draw.cc
  22. +59 −4 libstage/model_fiducial.cc
  23. +3 −3 libstage/model_getset.cc
  24. +3 −3 libstage/model_gripper.cc
  25. +10 −3 libstage/model_laser.cc
  26. +2 −0  libstage/model_load.cc
  27. +1 −1  libstage/model_ranger.cc
  28. +8 −0 libstage/stage.cc
  29. +62 −26 libstage/stage.hh
  30. +52 −5 libstage/world.cc
  31. +4 −2 libstage/worldgui.cc
  32. 0  tests/worldfile/test.py
  33. 0  worlds/cfggen.sh
  34. 0  worlds/test.sh
  35. 0  worlds/worldgen.sh
View
0  docsrc/Markdown.pl 100755 → 100644
File mode changed
View
0  docsrc/sourcedocs.sh 100755 → 100644
File mode changed
View
0  docsrc/upload.sh 100755 → 100644
File mode changed
View
14 examples/ctrl/CMakeLists.txt
@@ -23,10 +23,9 @@ 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}" )
@@ -38,15 +37,6 @@ endforeach( PLUGIN )
# delete the "lib" prefix from the plugin libraries
SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
-# need plaer's wavefront planning library for this one
-#if( PLAYER_FOUND )
-# link_directories( ${PLAYER_LIBRARY_DIRS} )
-# include_directories( ${PLAYER_INCLUDE_DIRS} )
-# target_link_libraries( fasr_plan "-lwavefront_standalone" )
-#endif( PLAYER_FOUND )
-
-
-
# install in <prefix>/lib
-install( TARGETS ${PLUGINS} fasr2 DESTINATION lib) #fasr3
+install( TARGETS ${PLUGINS} fasr2 DESTINATION lib)
View
0  examples/ctrl/astar/findpath.cpp 100755 → 100644
File mode changed
View
0  examples/ctrl/astar/fsa.h 100755 → 100644
File mode changed
View
0  examples/ctrl/astar/license.txt 100755 → 100644
File mode changed
View
0  examples/ctrl/astar/readme.txt 100755 → 100644
File mode changed
View
0  examples/ctrl/astar/stlastar.h 100755 → 100644
File mode changed
View
0  examples/libplayerc/playerswarm/Makefile 100755 → 100644
File mode changed
View
0  examples/libplayerc/playerswarm/args.h 100755 → 100644
File mode changed
View
0  examples/libplayerc/playerswarm/many.sh 100755 → 100644
File mode changed
View
333 libstage/#blockgroup.cc#
@@ -0,0 +1,333 @@
+
+#include "stage.hh"
+#include "worldfile.hh"
+
+#include <libgen.h> // for dirname(3)
+#include <limits.h> // 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; r<rect_count; r++ )
+ {
+ stg_point_t pts[4];
+
+ double x = rects[r].pose.x;
+ double y = rects[r].pose.y;
+ double w = rects[r].size.x;
+ double h = rects[r].size.y;
+
+ 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;
+
+ AppendBlock( new Block( mod,
+ pts,4,
+ 0,1,
+ col,
+ true ) );
+ }
+ free( rects );
+ }
+
+ CalcSize();
+}
+
+
+void BlockGroup::Rasterize( uint8_t* data,
+ unsigned int width,
+ unsigned int height,
+ stg_meters_t cellwidth,
+ stg_meters_t cellheight )
+{
+ FOR_EACH( it, blocks )
+ (*it)->Rasterize( data, width, height, cellwidth, cellheight );
+}
View
7 libstage/CMakeLists.txt
@@ -49,9 +49,9 @@ 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}
+# ${OPENGL_LIBRARIES}
+# ${FLTK_LIBS2}
)
# causes the shared library to have a version number
@@ -64,9 +64,6 @@ set( stagebinarySrcs main.cc )
add_executable( stagebinary ${stagebinarySrcs} )
-# When compiling stagebinarySrcs, pass CFLAGS from fltk-config
-set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
-
# 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
View
44 libstage/block.cc
@@ -11,12 +11,13 @@ static void canonicalize_winding(vector<stg_point_t>& 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 );
}
/////////////////////////////////////////////////////////////////////////////////////////
View
9 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 );
}
View
1  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);
}
View
7 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;
}
View
29 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 )
{
View
2  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 );
View
79 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<Model*>::iterator xmin = world->models_with_fiducials_byx.lower_bound( &left );
+// std::set<Model*>::iterator xmax = world->models_with_fiducials_byx.upper_bound( &right );
+
+// std::set<Model*>::iterator ymin = world->models_with_fiducials_byy.lower_bound( &down );
+// std::set<Model*>::iterator ymax = world->models_with_fiducials_byy.upper_bound( &up );
+
+
+// PushColor( Color(1,0,0,0.5) );
+
+
+// std::vector<Model*> candidates;
+// std::set<Model*> 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();
+
}
View
63 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<Model*,World::ltx>::iterator xmin =
+ world->models_with_fiducials_byx.lower_bound( &edge );
+
+ edge.pose = Pose( gp.x+rng, gp.y, 0, 0 ); // RIGHT
+ const std::set<Model*,World::ltx>::iterator xmax =
+ world->models_with_fiducials_byx.upper_bound( &edge );
+
+ edge.pose = Pose( gp.x, gp.y-rng, 0, 0 ); // BOTTOM
+ std::set<Model*,World::lty>::iterator ymin =
+ world->models_with_fiducials_byy.lower_bound( &edge );
+
+ edge.pose = Pose( gp.x, gp.y+rng, 0, 0 ); // TOP
+ const std::set<Model*,World::lty>::iterator ymax =
+ world->models_with_fiducials_byy.upper_bound( &edge );
+
+ // put these models into sets keyed on model pointer, rather than position
+ std::set<Model*> 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 closeby
+ std::vector<Model*> candidates;
+ std::set_intersection( horiz.begin(), horiz.end(),
+ vert.begin(), vert.end(),
+ std::inserter( candidates, candidates.end() ) );
- FOR_EACH( it, world->models_with_fiducials )
- AddModelIfVisible( *it );
+ // printf( "cand sz %lu\n", candidates.size() );
+
+ // create sets sorted by x and y position
+ FOR_EACH( it, candidates ) //world->models_with_fiducials )
+ 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();
}
View
6 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 );
}
View
6 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
View
13 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); t<sample_count; t += resolution )
{
- stg_raytrace_result_t r( world->Raytrace( 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::Sample>& ModelLaser::GetSamples() const
return samples;
}
+std::vector<ModelLaser::Sample>& ModelLaser::GetSamples()
+{
+ return samples;
+}
+
// VIS -------------------------------------------------------------------
View
2  libstage/model_load.cc
@@ -229,6 +229,8 @@ void Model::Load()
}
+ interval = 1000 * wf->Readint( wf_entity, "update_interval", interval/1000 );
+
Say( wf->ReadString(wf_entity, "say", "" ));
View
2  libstage/model_ranger.cc
@@ -265,7 +265,7 @@ void ModelRanger::Update( void )
//for( int r=0; r<sensors[t].ray_count; r++ )
//{
- stg_raytrace_result_t ray = Raytrace( s.pose,
+ RaytraceResult ray = Raytrace( s.pose,
s.bounds_range.max,
ranger_match,
NULL );
View
8 libstage/stage.cc
@@ -49,6 +49,14 @@ bool Stg::InitDone()
return init_called;
}
+const Color Color::blue( 0,0,1 );
+const Color Color::red( 1,0,0 );
+const Color Color::green( 0,1,0 );
+const Color Color::yellow( 1,1,0 );
+const Color Color::magenta( 1,0,1 );
+const Color Color::cyan( 0,1,1 );
+
+
static inline uint8_t* pb_get_pixel( Fl_Shared_Image* img, int x, int y )
{
uint8_t* pixels = (uint8_t*)(img->data()[0]);
View
88 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<Model*,ltx> models_with_fiducials_byx;
+ std::set<Model*,lty> 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 );
@@ -1154,12 +1168,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 +1238,8 @@ namespace Stg
Bounds local_z; ///< z extent in local coords
Color color;
bool inherit_color;
-
+ bool wheel;
+
void DrawTop();
void DrawSides();
@@ -1691,7 +1707,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 +2046,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 +2059,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 +2074,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 +2151,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 +2407,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
+
+
};
@@ -2471,7 +2502,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 +2533,7 @@ namespace Stg
unsigned int sample_count;
std::vector<Sample> samples;
+ public:
stg_meters_t range_max;
stg_radians_t fov;
uint32_t resolution;
@@ -2527,6 +2559,9 @@ namespace Stg
/** returns a const reference to a vector of range and reflectance samples */
const std::vector<Sample>& GetSamples() const;
+ /** returns a mutable reference to a vector of range and reflectance samples */
+ std::vector<Sample>& GetSamples();
+
/** Get the user-tweakable configuration of the laser */
Config GetConfig( ) const;
@@ -2678,6 +2713,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.
View
57 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();
View
6 libstage/worldgui.cc
@@ -70,7 +70,7 @@ location of the center of the window in world coordinates (meters)
- rotate [ <pitch:float> <yaw:float> ]\n
angle relative to straight up, angle of rotation (degrees)
- scale <float>\n
-ratio of world to pixel coordinates (window zoom)
+ ratio of world to pixel coordinates (window zoom)
- pcam_loc [ <x:int> <y:int> <z:int> ]\n
location of the perspective camera (meters)
- pcam_angle [ <pitch:float> <yaw:float> ]\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;
}
View
0  tests/worldfile/test.py 100755 → 100644
File mode changed
View
0  worlds/cfggen.sh 100755 → 100644
File mode changed
View
0  worlds/test.sh 100755 → 100644
File mode changed
View
0  worlds/worldgen.sh 100755 → 100644
File mode changed
Please sign in to comment.
Something went wrong with that request. Please try again.