Revision: 7782
http://playerstage.svn.sourceforge.net/playerstage/?rev=7782&view=rev
Author: rtv
Date: 2009-06-04 20:35:20 +0000 (Thu, 04 Jun 2009)
Log Message:
-----------
API clean up and small speed-ups
Modified Paths:
--------------
code/stage/trunk/CMakeLists.txt
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_fiducial.cc
code/stage/trunk/libstage/model_getset.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/libstageplugin/CMakeLists.txt
code/stage/trunk/libstageplugin/p_fiducial.cc
Modified: code/stage/trunk/CMakeLists.txt
===================================================================
--- code/stage/trunk/CMakeLists.txt 2009-06-04 14:21:36 UTC (rev 7781)
+++ code/stage/trunk/CMakeLists.txt 2009-06-04 20:35:20 UTC (rev 7782)
@@ -45,11 +45,11 @@
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" )
-ELSE (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND")
- message( STATUS " found" )
-ENDIF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND")
+#IF (${LTDL_LIB} STREQUAL "LTDL_LIB-NOTFOUND")
+# 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")
include_directories(
${OPENGL_INCLUDE_DIR}
@@ -64,24 +64,23 @@
SET( INDENT " * " )
pkg_search_module( GLIB REQUIRED gthread-2.0 )
IF( GLIB_FOUND )
- MESSAGE( STATUS ${INDENT} "Glib version ${GLIB_VERSION} detected at
${GLIB_PREFIX}" )
- MESSAGE( STATUS " GLIB_CFLAGS = ${GLIB_CFLAGS}" )
- MESSAGE( STATUS " GLIB_LDFLAGS = ${GLIB_LDFLAGS}" )
+# MESSAGE( STATUS ${INDENT} "Glib version ${GLIB_VERSION} detected at
${GLIB_PREFIX}" )
+# MESSAGE( STATUS " GLIB_CFLAGS = ${GLIB_CFLAGS}" )
+# MESSAGE( STATUS " GLIB_LDFLAGS = ${GLIB_LDFLAGS}" )
ELSE( GLIB_FOUND )
MESSAGE( ${INDENT} "Glib not detected" )
ENDIF( GLIB_FOUND )
pkg_search_module( LIBPNG REQUIRED libpng )
IF( LIBPNG_FOUND )
- MESSAGE( STATUS ${INDENT} "Libpng version ${LIBPNG_VERSION} detected at
${LIBPNG_PREFIX}" )
- MESSAGE( STATUS " LIBPNG_CFLAGS = ${LIBPNG_CFLAGS}" )
- MESSAGE( STATUS " LIBPNG_LDFLAGS = ${LIBPNG_LDFLAGS}" )
+# MESSAGE( STATUS ${INDENT} "Libpng version ${LIBPNG_VERSION} detected at
${LIBPNG_PREFIX}" )
+# MESSAGE( STATUS " LIBPNG_CFLAGS = ${LIBPNG_CFLAGS}" )
+# MESSAGE( STATUS " LIBPNG_LDFLAGS = ${LIBPNG_LDFLAGS}" )
ELSE( LIBPNG_FOUND )
MESSAGE( ${INDENT} "Libpng not detected" )
ENDIF( LIBPNG_FOUND )
-MESSAGE( STATUS "BUILD_GUI is ${BUILD_GUI}" )
-
+#MESSAGE( STATUS "BUILD_GUI is ${BUILD_GUI}" )
#IF( BUILD_GUI )
## the FLTK package script is not useful - it finds an X11-based FLTK on OS X.
@@ -109,7 +108,7 @@
MESSAGE (STATUS "Linking to FLTK statically.")
ELSE (LINK_FLTK_STATICALLY)
SET (FLTK_CONFIG_CMD "--ldflags")
- MESSAGE (STATUS "Linking to FLTK dynamically.")
+ #MESSAGE (STATUS "Linking to FLTK dynamically.")
ENDIF (LINK_FLTK_STATICALLY)
EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --version
@@ -128,9 +127,9 @@
# replace the newlines with semicolons
STRING(REGEX REPLACE "\n" ";" FLTK_LIBS "${FLTK_LIBS_NEWLINES}")
-MESSAGE( STATUS " FLTK_CFLAGS = ${FLTK_CFLAGS}")
-MESSAGE( STATUS " FLTK_LDFLAGS = ${FLTK_LDFLAGS}")
-MESSAGE( STATUS " FLTK_LIBS = ${FLTK_LIBS}")
+#MESSAGE( STATUS " FLTK_CFLAGS = ${FLTK_CFLAGS}")
+#MESSAGE( STATUS " FLTK_LDFLAGS = ${FLTK_LDFLAGS}")
+#MESSAGE( STATUS " FLTK_LIBS = ${FLTK_LIBS}")
SET (FLTK_FOUND TRUE)
@@ -141,9 +140,9 @@
IF( NOT OPENGL_GLU_FOUND )
MESSAGE( FATAL_ERROR "OpenGL GLU not found, aborting" )
ENDIF( NOT OPENGL_GLU_FOUND )
-MESSAGE( STATUS ${INDENT} "OpenGL GLU found at ${OPENGL_INCLUDE_DIR}" )
-MESSAGE( STATUS " OPENGL_INCLUDE_DIR = ${OPENGL_INCLUDE_DIR}")
-MESSAGE( STATUS " OPENGL_glu_LIBRARY = ${OPENGL_glu_LIBRARY}")
+#MESSAGE( STATUS ${INDENT} "OpenGL GLU found at ${OPENGL_INCLUDE_DIR}" )
+#MESSAGE( STATUS " OPENGL_INCLUDE_DIR = ${OPENGL_INCLUDE_DIR}")
+#MESSAGE( STATUS " OPENGL_glu_LIBRARY = ${OPENGL_glu_LIBRARY}")
MESSAGE( STATUS "Checking for optional libraries..." )
@@ -151,8 +150,8 @@
pkg_search_module( PLAYER playercore>=2.1.0 )
IF( PLAYER_FOUND )
MESSAGE( STATUS ${INDENT} "Player version ${PLAYER_VERSION} detected at
${PLAYER_PREFIX}" )
- MESSAGE( STATUS " PLAYER_CFLAGS: ${PLAYER_CFLAGS}" )
- MESSAGE( STATUS " PLAYER_LDFLAGS: ${PLAYER_LDFLAGS}" )
+# 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." )
ENDIF( PLAYER_FOUND )
@@ -160,8 +159,8 @@
pkg_search_module( WEBSIM websim )
IF( WEBSIM_FOUND )
MESSAGE( STATUS ${INDENT} "WebSim version ${WEBSIM_VERSION} detected at
${WEBSIM_PREFIX}" )
- MESSAGE( STATUS " WEBSIM_CFLAGS = ${WEBSIM_CFLAGS}" )
- MESSAGE( STATUS " WEBSIM_LDFLAGS = ${WEBSIM_LDFLAGS}" )
+# MESSAGE( STATUS " WEBSIM_CFLAGS = ${WEBSIM_CFLAGS}" )
+# MESSAGE( STATUS " WEBSIM_LDFLAGS = ${WEBSIM_LDFLAGS}" )
ELSE( WEBSIM_FOUND )
MESSAGE( ${INDENT} "WebSim not detected." )
ENDIF( WEBSIM_FOUND )
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-06-04 14:21:36 UTC (rev
7781)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-06-04 20:35:20 UTC (rev
7782)
@@ -425,26 +425,26 @@
static int FiducialUpdate( ModelFiducial* mod, Robot* robot )
{
- robot->charger_ahoy = false;
-
- for( unsigned int i = 0; i < mod->fiducial_count; i++ )
- {
- stg_fiducial_t* f = &mod->fiducials[i];
+ robot->charger_ahoy = false;
+ std::vector<ModelFiducial::Fiducial>& fids =
mod->GetFiducials();
+
+ for( unsigned int i = 0; i < fids.size(); i++ )
+ {
//printf( "fiducial %d is %d at %.2f m %.2f radians\n",
// i, f->id, f->range, f->bearing );
- if( f->id == 2 ) // I see a charging station
- {
- // record that I've seen it and where it is
- robot->charger_ahoy = true;
- robot->charger_bearing = f->bearing;
- robot->charger_range = f->range;
- robot->charger_heading = f->geom.a;
-
- //printf( "charger at %.2f radians\n",
robot->charger_bearing );
- break;
- }
+ if( fids[i].id == 2 ) // I see a charging station
+ {
+ // record that I've seen it and where
it is
+ robot->charger_ahoy = true;
+ robot->charger_bearing =
fids[i].bearing;
+ robot->charger_range = fids[i].range;
+ robot->charger_heading = fids[i].geom.a;
+
+ //printf( "charger at %.2f radians\n",
robot->charger_bearing );
+ break;
+ }
}
return 0; // run again
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-06-04 14:21:36 UTC (rev 7781)
+++ code/stage/trunk/libstage/model.cc 2009-06-04 20:35:20 UTC (rev 7782)
@@ -559,11 +559,6 @@
return candidate->IsDescendent( that );
}
-// Pose Model::LocalToGlobal( const Pose& pose ) const
-// {
-// return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
-// }
-
stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const
{
Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) );
@@ -847,19 +842,15 @@
blockgroup.SwitchToTestedCells();
}
-bool collisions_enabled( false );
-
Model* Model::ConditionalMove( const Pose& newpose )
{
assert( newpose.a >= -M_PI );
assert( newpose.a <= M_PI );
- Pose startpose = pose;
+ Pose startpose( pose );
pose = newpose; // do the move provisionally - we might undo it below
- // Model* hitmod = collisions_enabled ? TestCollisionTree() : NULL;
-
- Model* hitmod = TestCollisionTree();
+ Model* hitmod( TestCollisionTree() );
if( hitmod )
pose = startpose; // move failed - put me back where I started
@@ -892,30 +883,21 @@
// g_array_append_val( this->trail, checkpoint );
// }
-
+
// convert usec to sec
- double interval = (double)world->interval_sim / 1e6;
-
+ double interval( (double)world->interval_sim / 1e6 );
+
// find the change of pose due to our velocity vector
- Pose p;
- p.x = velocity.x * interval;
- p.y = velocity.y * interval;
- p.z = velocity.z * interval;
- p.a = normalize( velocity.a * interval );
-
- //if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) )
- //printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n",
- // token, p.x, p.y, p.z, p.a );
-
+ Pose p( velocity.x * interval,
+ velocity.y * interval,
+ velocity.z * interval,
+ normalize( velocity.a * interval ));
+
// attempts to move to the new pose. If the move fails because we'd
- // hit another model, that model is returned.
- Pose q = pose_sum( pose, p );
- assert( q.a >= -M_PI );
- assert( q.a <= M_PI );
-
- Model* hitthing = ConditionalMove( q );
-
- SetStall( hitthing ? true : false );
+ // hit another model, that model is returned.
+ // ConditionalMove() returns a pointer to the model we hit, or
+ // NULL. We use this as a boolean for SetStall()
+ SetStall( ConditionalMove( pose_sum( pose, p ) ) );
}
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2009-06-04 14:21:36 UTC (rev
7781)
+++ code/stage/trunk/libstage/model_fiducial.cc 2009-06-04 20:35:20 UTC (rev
7782)
@@ -25,7 +25,7 @@
const stg_watts_t DEFAULT_FIDUCIAL_WATTS = 10.0;
//TODO make instance attempt to register an option (as customvisualizations do)
-Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "",
false, NULL );
+Option ModelFiducial::showFiducialData( "Fiducials", "show_fiducial", "",
false, NULL );
/**
@ingroup model
@@ -68,7 +68,14 @@
ModelFiducial::ModelFiducial( World* world,
Model* parent )
- : Model( world, parent, MODEL_TYPE_FIDUCIAL )
+ : Model( world, parent, MODEL_TYPE_FIDUCIAL ),
+ fiducials(),
+ max_range_anon( DEFAULT_FIDUCIAL_RANGEMAXANON ),
+ max_range_id( DEFAULT_FIDUCIAL_RANGEMAXID ),
+ min_range( DEFAULT_FIDUCIAL_RANGEMIN ),
+ fov( DEFAULT_FIDUCIAL_FOV ),
+ heading( 0 ),
+ key( 0 )
{
//PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n",
// id, typestr );
@@ -84,25 +91,12 @@
Geom geom;
memset( &geom, 0, sizeof(geom));
SetGeom( geom );
- fiducials = NULL;
- fiducial_count = 0;
- // default parameters
- min_range = DEFAULT_FIDUCIAL_RANGEMIN;
- max_range_anon = DEFAULT_FIDUCIAL_RANGEMAXANON;
- max_range_id = DEFAULT_FIDUCIAL_RANGEMAXID;
- fov = DEFAULT_FIDUCIAL_FOV;
- key = 0;
-
- data = g_array_new( false, true, sizeof(stg_fiducial_t) );
-
RegisterOption( &showFiducialData );
}
ModelFiducial::~ModelFiducial( void )
{
- if( data )
- g_array_free( data, true );
}
static bool fiducial_raytrace_match( Model* candidate,
@@ -187,7 +181,7 @@
Geom hisgeom = him->GetGeom();
// record where we saw him and what he looked like
- stg_fiducial_t fid;
+ Fiducial fid;
fid.mod = him;
fid.range = range;
fid.bearing = dtheta;
@@ -196,7 +190,7 @@
fid.geom.a = normalize( hispose.a - mypose.a);
// store the global pose of the fiducial (mainly for the GUI)
- memcpy( &fid.pose, &hispose, sizeof(fid.pose));
+ fid.pose = hispose;
// if he's within ID range, get his fiducial.return value, else
// we see value 0
@@ -205,11 +199,8 @@
PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
him->Token(),
him->vis.fiducial_return );
- g_array_append_val( data, fid );
- }
-
- fiducials = (stg_fiducial_t*)data->data;
- fiducial_count = data->len;
+ fiducials.push_back( fid );
+ }
}
///////////////////////////////////////////////////////////////////////////
@@ -223,21 +214,14 @@
if( subs < 1 )
return;
-
+
// reset the array of detected fiducials
- data = g_array_set_size( data, 0 );
-
- // for all the objects in the world, test if we can see them as
- // fiducials
-
- // TODO - add a fiducial-only hash table to the world to speed this
- // up a lot for large populations
- world->ForEachDescendant(
(stg_model_callback_t)(ModelFiducial::AddModelIfVisibleStatic),
- this );
-
- PRINT_DEBUG2( "model %s saw %d fiducials", token, data->len );
- fiducials = (stg_fiducial_t*)data->data;
- fiducial_count = data->len;
+ fiducials.clear();
+
+ for( std::set<Model*>::iterator it =
world->models_with_fiducials.begin();
+ it != world->models_with_fiducials.end();
+ ++it )
+ AddModelIfVisible( *it );
}
void ModelFiducial::Load( void )
@@ -258,74 +242,70 @@
{
if ( !showFiducialData )
return;
+
+ PushColor( 1,0,1,0.7 ); // magenta, with a bit of alpha
// draw the FOV
-// GLUquadric* quadric = gluNewQuadric();
+// GLUquadric* quadric = gluNewQuadric();
+// gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
+
+// gluPartialDisk( quadric,
+// 0,
+//
max_range_anon,
+// 20, //
slices
+// 1, //
loops
+// rtod(
M_PI/2.0 + fov/2.0), // start angle
+//
rtod(-fov) ); // sweep angle
+
+// gluDeleteQuadric( quadric );
-// PushColor( 0,0,0,0.2 );
+ // draw fuzzy dotted lines
+ glLineWidth( 2.0 );
+ glLineStipple( 1, 0x00FF );
-// gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
+ // draw lines to the fiducials
+ for( unsigned int f=0; f<fiducials.size(); f++ )
+ {
+ Fiducial& fid = fiducials[f];
+
+ double dx = fid.range * cos( fid.bearing);
+ double dy = fid.range * sin( fid.bearing);
+
+
+ glEnable(GL_LINE_STIPPLE);
+ glBegin( GL_LINES );
+ glVertex2f( 0,0 );
+ glVertex2f( dx, dy );
+ glEnd();
+ glDisable(GL_LINE_STIPPLE);
+
+ PushColor( 1,0,1,1 ); // magenta, no alpha
-// gluPartialDisk( quadric,
-// 0,
-// max_range_anon,
-// 20, // slices
-// 1, // loops
-// rtod( M_PI/2.0 + fov/2.0), // start angle
-// rtod(-fov) ); // sweep angle
+ glPushMatrix();
+ Gl::coord_shift( dx,dy,0,fid.geom.a );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
+ fid.geom.x/2.0,
fid.geom.y/2.0 );
+
+ // show the fiducial ID
+ char idstr[32];
+ snprintf(idstr, 31, "%d", fid.id );
+ Gl::draw_string( 0,0,0, idstr );
-// gluDeleteQuadric( quadric );
-// PopColor();
-
- if( data->len == 0 )
- return;
-
- // draw the fiducials
-
- for( unsigned int f=0; f<data->len; f++ )
- {
- stg_fiducial_t fid = g_array_index( data, stg_fiducial_t, f );
-
- PushColor( 0,0,0,0.7 );
-
- double dx = fid.range * cos( fid.bearing);
- double dy = fid.range * sin( fid.bearing);
-
- glBegin( GL_LINES );
- glVertex2f( 0,0 );
- glVertex2f( dx, dy );
- glEnd();
-
- glPushMatrix();
- Gl::coord_shift( dx,dy,0,fid.geom.a );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
- fid.geom.x/2.0, fid.geom.y/2.0 );
-
- // show the fiducial ID
- char idstr[32];
- snprintf(idstr, 31, "%d", fid.id );
-
- PushColor( 0,0,0,1 ); // black
- Gl::draw_string( 0,0,0, idstr );
- PopColor();
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- glPopMatrix();
-
- PopColor();
- }
+ PopColor();
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ glPopMatrix();
+ }
+
+ PopColor();
+ glLineWidth( 1.0 );
}
-
+
void ModelFiducial::Shutdown( void )
{
PRINT_DEBUG( "fiducial shutdown" );
-
- // clear the data
- data = g_array_set_size( data, 0 );
- fiducials = NULL;
- fiducial_count = 0;
-
+ fiducials.clear();
Model::Shutdown();
}
Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc 2009-06-04 14:21:36 UTC (rev
7781)
+++ code/stage/trunk/libstage/model_getset.cc 2009-06-04 20:35:20 UTC (rev
7782)
@@ -45,6 +45,14 @@
void Model::SetFiducialReturn( int val )
{
vis.fiducial_return = val;
+
+ // non-zero values mean we need to be in the world's set of
+ // detectable models
+ if( val == 0 )
+ world->models_with_fiducials.erase( this );
+ else
+ world->models_with_fiducials.insert( this );
+
CallCallbacks( &vis.fiducial_return );
}
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-06-04 14:21:36 UTC (rev
7781)
+++ code/stage/trunk/libstage/model_laser.cc 2009-06-04 20:35:20 UTC (rev
7782)
@@ -284,7 +284,6 @@
void ModelLaser::Update( void )
{
assert( samples.size() == sample_count );
- //assert( rays.size() == sample_count );
double bearing = -fov/2.0;
// make the first and last rays exactly at the extremes of the FOV
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-06-04 14:21:36 UTC (rev
7781)
+++ code/stage/trunk/libstage/model_load.cc 2009-06-04 20:35:20 UTC (rev
7782)
@@ -179,8 +179,10 @@
if( wf->PropertyExists( wf_entity, "mass" ))
this->SetMass( wf->ReadFloat(wf_entity, "mass", this->mass ));
+
+ vis.Load( wf, wf_entity );
+ SetFiducialReturn( vis.fiducial_return ); // may have some work to do
- vis.Load( wf, wf_entity );
gui.Load( wf, wf_entity );
if( wf->PropertyExists( wf_entity, "map_resolution" ))
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-06-04 14:21:36 UTC (rev 7781)
+++ code/stage/trunk/libstage/region.hh 2009-06-04 20:35:20 UTC (rev 7782)
@@ -7,6 +7,9 @@
#include "stage.hh"
+#include <algorithm>
+#include <vector>
+
namespace Stg
{
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-06-04 14:21:36 UTC (rev 7781)
+++ code/stage/trunk/libstage/stage.hh 2009-06-04 20:35:20 UTC (rev 7782)
@@ -46,9 +46,10 @@
#include <vector>
#include <list>
#include <map>
+#include <set>
-// we use GLib's data structures extensively. Perhaps we'll move to
-// C++ STL types to lose this dependency one day.
+// we use GLib's data structures extensively. Gradually we're moving
+// to C++ STL types. May be able to lose this dependency one day.
#include <glib.h>
// FLTK Gui includes
@@ -597,7 +598,7 @@
}
/** returns a new pose, with each axis scaled */
- Pose pose_scale( const Pose& p1, const double x, const double y, const
double z );
+ inline Pose pose_scale( const Pose& p1, const double x, const double y,
const double z );
// PRETTY PRINTING -------------------------------------------------
@@ -663,7 +664,7 @@
typedef int(*stg_model_callback_t)(Model* mod, void* user );
typedef int(*stg_world_callback_t)(World* world, void* user );
- typedef int(*stg_cell_callback_t)(Cell* cell, void* user );
+ //typedef int(*stg_cell_callback_t)(Cell* cell, void* user );
// return val, or minval if val < minval, or maxval if val > maxval
double constrain( double val, double minval, double maxval );
@@ -899,8 +900,9 @@
/// %World class
class World : public Ancestor
{
- friend class Model; // allow access to private members
friend class Block;
+ friend class Model; // allow access to private members
+ friend class ModelFiducial;
friend class Canvas;
private:
@@ -931,7 +933,11 @@
GList* velocity_list; ///< Models with non-zero velocity and should have
their poses updated
unsigned int worker_threads; ///< the number of worker threads to use
GCond* worker_threads_done; ///< signalled when there are no more updates
for the worker threads to do
-
+
+ /** Keep a list of all models with detectable fiducials. This
+ avoids searching the whole world for fiducials.
*/
+ std::set<Model*> models_with_fiducials;
+
protected:
std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List
of callback functions and arguments
@@ -1148,22 +1154,6 @@
~Block();
-
-// bool RayTest( const stg_ray_test_func_t func,
-// const Model* mod,
-// const void* arg,
-// const bool ztest,
-// const stg_meters_t z )
-// {
-// // optionally test z is in right range
-// if( ztest && ( z < global_z.min ||
-// z > global_z.max ) )
-// return false;
-
-// // test the predicate we were passed
-// return( (*func)( this->mod, (Model*)mod, arg ));
-// }
-
/** render the block into the world's raytrace data structure */
void Map();
@@ -1696,7 +1686,7 @@
bool ranger_return;
Visibility();
- void Load( Worldfile* wf, int wf_entity );
+ void Load( Worldfile* wf, int wf_entity );
};
@@ -2369,10 +2359,10 @@
/** Return the global pose (i.e. pose in world coordinates) of a
pose specified in the model's local coordinate system */
Pose LocalToGlobal( const Pose& pose ) const
- {
- return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
+ {
+ return pose_sum( pose_sum( GetGlobalPose(), geom.pose
), pose );
}
-
+
// /** Return the 3d point in world coordinates of a 3d point
// specified in the model's local coordinate system */
// stg_point3_t LocalToGlobal( const stg_point3_t local ) const;
@@ -2541,9 +2531,7 @@
void SetConfig( Config& cfg );
};
- // \todo GRIPPER MODEL
--------------------------------------------------------
-
-
+ // GRIPPER MODEL --------------------------------------------------------
class ModelGripper : public Model
{
public:
@@ -2655,71 +2643,59 @@
// FIDUCIAL MODEL --------------------------------------------------------
- /** fiducial config packet
- */
- typedef struct
- {
- stg_meters_t max_range_anon;///< maximum detection range
- stg_meters_t max_range_id; ///< maximum range at which the ID can be
read
- stg_meters_t min_range; ///< minimum detection range
- stg_radians_t fov; ///< field of view
- stg_radians_t heading; ///< center of field of view
-
- /// only detects fiducials with a key string that matches this one
- /// (defaults to NULL)
- int key;
- } stg_fiducial_config_t;
-
- /** fiducial data packet
- */
- typedef struct
- {
- 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; ///< 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.
- } stg_fiducial_t;
-
/// %ModelFiducial class
class ModelFiducial : public Model
{
- private:
+ public:
+ /** Detected fiducial data */
+ class Fiducial
+ {
+ public:
+ 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; ///< 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.
+ };
+
+private:
// if neighbor is visible, add him to the fiducial scan
void AddModelIfVisible( Model* him );
- // static wrapper function can be used as a function pointer
- static int AddModelIfVisibleStatic( Model* him, ModelFiducial* me )
- { if( him != me ) me->AddModelIfVisible( him ); return 0; }
-
virtual void Update();
virtual void DataVisualize( Camera* cam );
- GArray* data;
-
static Option showFiducialData;
-
+
+ std::vector<Fiducial> fiducials;
+
public:
- static const char* typestr;
- // constructor
- ModelFiducial( World* world,
- Model* parent );
- // destructor
- virtual ~ModelFiducial();
-
- virtual void Load();
- void Shutdown( void );
+ static const char* typestr;
+
+ ModelFiducial( World* world, Model* parent );
+ virtual ~ModelFiducial();
+
+ virtual void Load();
+ void Shutdown( void );
- stg_meters_t max_range_anon;///< maximum detection range
- stg_meters_t max_range_id; ///< maximum range at which the ID can be
read
- stg_meters_t min_range; ///< minimum detection range
- stg_radians_t fov; ///< field of view
- stg_radians_t heading; ///< center of field of view
- int key; ///< /// only detect fiducials with a key that matches this
one (defaults 0)
-
- stg_fiducial_t* fiducials; ///< array of detected fiducials
- uint32_t fiducial_count; ///< the number of fiducials detected
+ stg_meters_t max_range_anon;///< maximum detection range
+ stg_meters_t max_range_id; ///< maximum range at which the ID
can be read
+ stg_meters_t min_range; ///< minimum detection range
+ stg_radians_t fov; ///< field of view
+ stg_radians_t heading; ///< center of field of view
+ int key; ///< /// only detect fiducials with a key that matches
this one (defaults 0)
+
+
+ /** Access the dectected fiducials. C++ style. */
+ std::vector<Fiducial>& GetFiducials() { return fiducials; }
+
+ /** Access the dectected fiducials, C style. */
+ Fiducial* GetFiducials( unsigned int* count )
+ {
+ if( count ) *count = fiducials.size();
+ return &fiducials[0];
+ }
};
@@ -2749,23 +2725,20 @@
public:
static const char* typestr;
- // constructor
- ModelRanger( World* world,
- Model* parent );
- // destructor
- virtual ~ModelRanger();
- virtual void Load();
- virtual void Print( char* prefix );
+ ModelRanger( World* world, Model* parent );
+ virtual ~ModelRanger();
+ virtual void Load();
+ virtual void Print( char* prefix );
+
std::vector<Sensor> sensors;
private:
- static Option showRangerData;
- static Option showRangerTransducers;
-
+ static Option showRangerData;
+ static Option showRangerTransducers;
};
-
+
// BLINKENLIGHT MODEL ----------------------------------------------------
class ModelBlinkenlight : public Model
{
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-06-04 14:21:36 UTC (rev 7781)
+++ code/stage/trunk/libstage/world.cc 2009-06-04 20:35:20 UTC (rev 7782)
@@ -82,6 +82,7 @@
velocity_list( NULL ),
worker_threads( 0 ),
worker_threads_done( g_cond_new() ),
+ models_with_fiducials(),
// protected
cb_list(NULL),
Modified: code/stage/trunk/libstageplugin/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-04 14:21:36 UTC
(rev 7781)
+++ code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-04 20:35:20 UTC
(rev 7782)
@@ -1,4 +1,4 @@
-MESSAGE( STATUS "Configuring Player plugin" )
+MESSAGE( STATUS "Configuring libstageplugin" )
link_directories( ${PLAYER_LIBRARY_DIRS} )
include_directories( ${PLAYER_INCLUDE_DIRS} )
Modified: code/stage/trunk/libstageplugin/p_fiducial.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_fiducial.cc 2009-06-04 14:21:36 UTC
(rev 7781)
+++ code/stage/trunk/libstageplugin/p_fiducial.cc 2009-06-04 20:35:20 UTC
(rev 7782)
@@ -61,38 +61,35 @@
player_fiducial_data_t pdata;
memset( &pdata, 0, sizeof(pdata) );
- ModelFiducial* fidmod = (ModelFiducial*)this->mod;
-
- if( fidmod->fiducial_count > 0 )
+ unsigned int count=0;
+ ModelFiducial::Fiducial* fids = ((ModelFiducial*)mod)->GetFiducials(
&count );
assert( fids );
+
+ if( count )
{
- stg_fiducial_t* fids = fidmod->fiducials;
- assert( fids );
-
- pdata.fiducials_count = fidmod->fiducial_count;
- pdata.fiducials = new
player_fiducial_item_t[pdata.fiducials_count];
-
- for( int i=0; i<(int)pdata.fiducials_count; i++ )
- {
- pdata.fiducials[i].id = fids[i].id;
-
- // 2D x,y only
- double xpos = fids[i].range * cos(fids[i].bearing);
- double ypos = fids[i].range * sin(fids[i].bearing);
-
- pdata.fiducials[i].pose.px = xpos;
- pdata.fiducials[i].pose.py = ypos;
- pdata.fiducials[i].pose.pz = 0.0;
- pdata.fiducials[i].pose.proll = 0.0;
- pdata.fiducials[i].pose.ppitch = 0.0;
- pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
- }
+ pdata.fiducials = new player_fiducial_item_t[count];
+
+ for( unsigned int i=0; i<count; i++ )
+ {
+ pdata.fiducials[i].id = fids[i].id;
+
+ // 2D x,y only
+ double xpos = fids[i].range *
cos(fids[i].bearing);
+ double ypos = fids[i].range *
sin(fids[i].bearing);
+
+ pdata.fiducials[i].pose.px = xpos;
+ pdata.fiducials[i].pose.py = ypos;
+ pdata.fiducials[i].pose.pz = 0.0;
+ pdata.fiducials[i].pose.proll = 0.0;
+ pdata.fiducials[i].pose.ppitch = 0.0;
+ pdata.fiducials[i].pose.pyaw =
fids[i].geom.a;
+ }
}
-
+
// publish this data
this->driver->Publish( this->addr,
-
PLAYER_MSGTYPE_DATA,
-
PLAYER_FIDUCIAL_DATA_SCAN,
- &pdata,
sizeof(pdata), NULL);
+
PLAYER_MSGTYPE_DATA,
+
PLAYER_FIDUCIAL_DATA_SCAN,
+
&pdata, sizeof(pdata), NULL);
if ( pdata.fiducials )
delete [] pdata.fiducials;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
OpenSolaris 2009.06 is a cutting edge operating system for enterprises
looking to deploy the next generation of Solaris that includes the latest
innovations from Sun and the OpenSource community. Download a copy and
enjoy capabilities such as Networking, Storage and Virtualization.
Go to: http://p.sf.net/sfu/opensolaris-get
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
