From: <rt...@us...> - 2008-12-07 02:07:26
|
Revision: 7191 http://playerstage.svn.sourceforge.net/playerstage/?rev=7191&view=rev Author: rtv Date: 2008-12-07 02:07:21 +0000 (Sun, 07 Dec 2008) Log Message: ----------- fixed tricky bug in fasr.cc when used in worlds bigger than 8m square. awful. Added some cautious NAN checks to avoid similar horror in future. Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -182,12 +182,23 @@ int x = (pose.x + 8) / 4; int y = (pose.y + 8) / 4; + + // oh what an awful bug - 5 hours to track this down. When using + // this controller in a world larger than 8*8 meters, a_goal can + // sometimes be NAN. Causing trouble WAY upstream. + if( x > 3 ) x = 3; + if( y > 3 ) y = 3; double a_goal = dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] ); + assert( ! isnan(a_goal ) ); + assert( ! isnan(pose.a ) ); + double a_error = normalize( a_goal - pose.a ); - + + assert( ! isnan(a_error) ); + robot->pos->SetTurnSpeed( a_error ); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/canvas.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -592,20 +592,25 @@ { stg_bounds3d_t bounds = world->GetExtent(); - char str[16]; - PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray - for( double i = floor(bounds.x.min); i < bounds.x.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( i, 0, 0.00, str ); - } +// printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n", +// bounds.x.min, bounds.x.max, +// bounds.y.min, bounds.y.max, +// bounds.z.min, bounds.z.max ); + + char str[64]; + PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray + for( double i = floor(bounds.x.min); i < bounds.x.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( i, 0, 0.00, str ); + } - for( double i = floor(bounds.y.min); i < bounds.y.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( 0, i, 0.00, str ); - } - PopColor(); + for( double i = floor(bounds.y.min); i < bounds.y.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( 0, i, 0.00, str ); + } + PopColor(); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/gl.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -21,10 +21,10 @@ } -// TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { glRasterPos3f( x, y, z ); + //printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str), str ); gl_draw(str); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/model.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -566,7 +566,22 @@ // should one day do all this with affine transforms for neatness? inline stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose ) { - return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); + stg_pose_t gpose = pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); + + if( isnan(gpose.x) || isnan(gpose.y) | isnan(gpose.z) || isnan(gpose.a ) ) + { + stg_pose_t g = GetGlobalPose(); + stg_pose_t p = parent ? parent->GetPose() : stg_pose_t(); + + printf( "model %p %s gpose BAD [%.2f %.2f %.2f %.2f] pose [%.2f %.2f %.2f %.2f] globalpose [%.2f %.2f %.2f %.2f] parent [%.2f %.2f %.2f %.2f] \n", + this, token, gpose.x, gpose.y, gpose.z, gpose.a, + pose.x, pose.y, pose.z, pose.a, + g.x, g.y, g.z, g.a, + p.x, p.y, p.z, p.a ); + return gpose; + + } + //return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } void StgModel::MapWithChildren() @@ -1223,6 +1238,11 @@ // vel.z, // vel.a ); + assert( ! isnan(vel.x) ); + assert( ! isnan(vel.y) ); + assert( ! isnan(vel.z) ); + assert( ! isnan(vel.a) ); + this->velocity = vel; if( on_velocity_list && velocity_is_zero( vel ) ) @@ -1250,28 +1270,24 @@ world->NeedRedraw(); } -void StgModel::SetPose( stg_pose_t pose ) +void StgModel::SetPose( stg_pose_t newpose ) { //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", // this->token, pose->x, pose->y, pose->z, pose->a ); // if the pose has changed, we need to do some work - if( memcmp( &this->pose, &pose, sizeof(stg_pose_t) ) != 0 ) + if( memcmp( &pose, &newpose, sizeof(stg_pose_t) ) != 0 ) { - //puts( "SETPOSE" ); - - // UnMapWithChildren(); + pose = newpose; + pose.a = normalize(pose.a); - - pose.a = normalize( pose.a ); - this->pose = pose; - this->pose.a = normalize(this->pose.a); - - this->NeedRedraw(); - - this->map_caches_are_invalid = true; + if( isnan( pose.a ) ) + printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", + token, pose.x, pose.y, pose.z, pose.a ); + + NeedRedraw(); + map_caches_are_invalid = true; MapWithChildren(); - world->dirty = true; } @@ -1483,6 +1499,10 @@ StgModel* StgModel::ConditionalMove( stg_pose_t newpose ) { + if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) + printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n", + token, newpose.x, newpose.y, newpose.z, newpose.a ); + stg_pose_t startpose = pose; pose = newpose; // do the move provisionally - we might undo it below @@ -1496,6 +1516,11 @@ world->dirty = true; // need redraw } + + if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a ) ) + printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n", + token, pose.x, pose.y, pose.z, pose.a ); + return hitmod; } @@ -1532,6 +1557,10 @@ p.z = velocity.z * interval; p.a = velocity.a * interval; + if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) ) + printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n", + token, p.x, p.y, p.z, p.a ); + // attempts to move to the new pose. If the move fails because we'd // hit another model, that model is returned. StgModel* hitthing = ConditionalMove( pose_sum( pose, p ) ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/model_laser.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -176,7 +176,7 @@ double sample_incr = fov / (double)(sample_count-1); samples = g_renew( stg_laser_sample_t, samples, sample_count ); - + stg_pose_t rayorg = geom.pose; bzero( &rayorg, sizeof(rayorg)); rayorg.z += geom.size.z/2; Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/model_position.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -472,41 +472,54 @@ void StgModelPosition::SetSpeed( double x, double y, double a ) { - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.x = x; - goal.y = y; - goal.z = 0; - goal.a = a; + assert( ! isnan(x) ); + assert( ! isnan(y) ); + assert( ! isnan(a) ); + + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.x = x; + goal.y = y; + goal.z = 0; + goal.a = a; } void StgModelPosition::SetXSpeed( double x ) { - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.x = x; + assert( ! isnan(x) ); + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.x = x; } void StgModelPosition::SetYSpeed( double y ) { - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.y = y; + assert( ! isnan(y) ); + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.y = y; } void StgModelPosition::SetZSpeed( double z ) { - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.z = z; + assert( ! isnan(z) ); + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.z = z; } void StgModelPosition::SetTurnSpeed( double a ) { - control_mode = STG_POSITION_CONTROL_VELOCITY; - goal.a = a; + assert( ! isnan(a) ); + control_mode = STG_POSITION_CONTROL_VELOCITY; + goal.a = a; } void StgModelPosition::SetSpeed( stg_velocity_t vel ) { + assert( ! isnan(vel.x) ); + assert( ! isnan(vel.y) ); + assert( ! isnan(vel.z) ); + assert( ! isnan(vel.a) ); + control_mode = STG_POSITION_CONTROL_VELOCITY; goal.x = vel.x; goal.y = vel.y; @@ -516,6 +529,10 @@ void StgModelPosition::GoTo( double x, double y, double a ) { + assert( ! isnan(x) ); + assert( ! isnan(y) ); + assert( ! isnan(a) ); + control_mode = STG_POSITION_CONTROL_POSITION; goal.x = x; goal.y = y; Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/region.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -14,7 +14,7 @@ Region::Region() - : count(0), cells(NULL) + : cells(NULL), count(0) { //for( unsigned int i=0; i<Region::SIZE; i++ ) //cells[i].region = this; @@ -52,18 +52,66 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - // outline regions + // outline superregion + glColor3f( 0,0,1 ); + glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); + + // outline regions glColor3f( 0,1,0 ); for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) - glRecti( x<<RBITS, y<<RBITS, - (x+1)<<RBITS, (y+1)<<RBITS ); + { + Region* r = GetRegion(x,y); - // outline superregion - glColor3f( 0,0,1 ); - - glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); - + if( r->count ) + // outline regions with contents + glRecti( x<<RBITS, y<<RBITS, + (x+1)<<RBITS, (y+1)<<RBITS ); + else if( r->cells ) + { + double left = x << RBITS; + double right = (x+1) << RBITS; + double bottom = y << RBITS; + double top = (y+1) << RBITS; + + double d = 3.0; + + // draw little corner markers for regions with memory + // allocated but no contents + glBegin( GL_LINES ); + + glVertex2f( left, bottom ); + glVertex2f( left+d, bottom ); + + glVertex2f( left, bottom ); + glVertex2f( left, bottom+d ); + + + glVertex2f( left, top ); + glVertex2f( left+d, top ); + + glVertex2f( left, top ); + glVertex2f( left, top-d ); + + + + glVertex2f( right, top ); + glVertex2f( right-d, top ); + + glVertex2f( right, top ); + glVertex2f( right, top-d ); + + + glVertex2f( right, bottom ); + glVertex2f( right-d, bottom ); + + glVertex2f( right, bottom ); + glVertex2f( right, bottom+d ); + + glEnd(); + } + } + char buf[32]; snprintf( buf, 15, "%lu", count ); gl_draw_string( 1<<SBITS, 1<<SBITS, 0, buf ); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/stage.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -52,7 +52,18 @@ return init_called; } +double Stg::normalize( double a ) +{ + assert( ! isnan(a) ); + + //return( atan2(sin(a), cos(a))); + // faster than return( atan2(sin(a), cos(a))); + while( a < -M_PI ) a += (2.0 * M_PI); + while( a > M_PI ) a -= (2.0 * M_PI); + return a; +}; + void Stg::RegisterModel( stg_model_type_t type, const char* name, stg_creator_t creator ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/stage.hh 2008-12-07 02:07:21 UTC (rev 7191) @@ -168,15 +168,7 @@ /** Normalize an angle to within +/_ M_PI */ - inline double normalize( double a ) - { - return( atan2(sin(a), cos(a))); - // faster than return( atan2(sin(a), cos(a))); - //while( a < -M_PI ) a += 2.0 * M_PI; - //while( a > M_PI ) a -= 2.0 * M_PI; - //return a; - } - + double normalize( double a ); /** take binary sign of a, either -1, or 1 if >= 0 */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-12-06 21:25:20 UTC (rev 7190) +++ code/stage/trunk/libstage/world.cc 2008-12-07 02:07:21 UTC (rev 7191) @@ -519,20 +519,22 @@ // inline functions for converting from global cell coordinates to // superregions, regions and local cells -inline int32_t SUPERREGION( const int32_t& x ) -{ +inline int32_t SUPERREGION( const int32_t x ) +{ + if( abs((x>>SRBITS) > 100 ) ) + printf( "MACRO[ %d %d ]", x, (x>>SRBITS) ); return( x >> SRBITS ); } -inline int32_t REGION( const int32_t& x ) +inline int32_t REGION( const int32_t x ) { const int32_t _region_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS ); return( ( x & _region_coord_mask ) >> RBITS ); } -inline int32_t CELL( const int32_t& x ) +inline int32_t CELL( const int32_t x ) { - static const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); + const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); return( x & _cell_coord_mask ); } @@ -568,14 +570,14 @@ const bool ztest ) { stg_raytrace_result_t sample; - - // printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n", - // pose.x, - // pose.y, - // pose.z, - // pose.a, - // range ); - + +// printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n", +// gpose.x, +// gpose.y, +// gpose.z, +// gpose.a, +// range ); + // initialize the sample sample.pose = gpose; sample.range = range; // we might change this below @@ -585,7 +587,7 @@ stg_point_int_t glob; glob.x = (int32_t)(gpose.x*ppm); glob.y = (int32_t)(gpose.y*ppm); - + // record our starting position stg_point_int_t start = glob; @@ -640,6 +642,18 @@ else if( dy > 0 ) n++; + if( abs(sup.x) > 20 ) + printf( "raytracing at [ %.2f %.2f %.2f %.2f ] GLOB( %d %d ) SUP( %d %d )\n", + gpose.x, + gpose.y, + gpose.z, + gpose.a, + glob.x, + glob.y, + sup.x, + sup.y ); + + // find the starting superregion sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely @@ -784,6 +798,7 @@ SuperRegion* StgWorld::AddSuperRegion( const stg_point_int_t& sup ) { //printf( "Creating super region [ %d %d ]\n", sup.x, sup.y ); + SuperRegion* sr = CreateSuperRegion( sup ); // the bounds of the world have changed @@ -815,6 +830,8 @@ inline SuperRegion* StgWorld::GetSuperRegion( const stg_point_int_t& sup ) { + //printf( "SUP[ %d %d ] ", sup.x, sup.y ); + // no, so we try to fetch it out of the hash table SuperRegion* sr = (SuperRegion*) g_hash_table_lookup( superregions, (gpointer)&sup ); @@ -835,6 +852,8 @@ glob.x = x; glob.y = y; + //printf( "GC[ %d %d ] ", glob.x, glob.y ); + return( GetSuperRegionCached( SUPERREGION(glob) ) ->GetRegion( REGION(x), REGION(y) ) ->GetCell( CELL(x), CELL(y) )) ; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-07 02:18:36
|
Revision: 7192 http://playerstage.svn.sourceforge.net/playerstage/?rev=7192&view=rev Author: rtv Date: 2008-12-07 02:18:32 +0000 (Sun, 07 Dec 2008) Log Message: ----------- working on new large-scale demo Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc Added Paths: ----------- code/stage/trunk/worlds/asr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2008-12-07 02:07:21 UTC (rev 7191) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-12-07 02:18:32 UTC (rev 7192) @@ -16,7 +16,7 @@ { 90, 180, 180, 180 }, { 90, -90, 0, -90 }, { 90, 90, 180, 90 }, - { 0, 45, 0, 45} + { 0, 45, 0, 0} }; double need[4][4] = { @@ -186,8 +186,10 @@ // oh what an awful bug - 5 hours to track this down. When using // this controller in a world larger than 8*8 meters, a_goal can // sometimes be NAN. Causing trouble WAY upstream. - if( x > 3 ) x = 3; - if( y > 3 ) y = 3; + if( x > 3 ) x = 3; + if( y > 3 ) y = 3; + if( x < 0 ) x = 0; + if( y < 0 ) y = 0; double a_goal = dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] ); Added: code/stage/trunk/worlds/asr.world =================================================================== --- code/stage/trunk/worlds/asr.world (rev 0) +++ code/stage/trunk/worlds/asr.world 2008-12-07 02:18:32 UTC (rev 7192) @@ -0,0 +1,161 @@ +# ASR demo world +# Authors: Richard Vaughan +# $Id: fasr.world,v 1.4 2008-04-01 23:57:41 rtv Exp $ + +include "pioneer.inc" +include "map.inc" +include "sick.inc" + +interval_sim 100 # simulation timestep in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds +paused 1 + +resolution 0.02 + +# threads may help here depending on your CPU +threadpool 0 +# threadpool 2 + + +# configure the GUI window +window +( + size [ 1916.000 1156.000 ] + + center [ 1.814 5.214 ] + rotate [ 0 0 ] + scale 71.081 + + pcam_loc [ 0 -4.000 2.000 ] + pcam_angle [ 70.000 0 ] + pcam_on 0 + + show_data 1 + show_flags 1 + + interval 50 +) + +# load an environment bitmap +floorplan +( + name "cave" + size [100.000 100.000 0.600] + pose [0 0 0 0] + bitmap "bitmaps/frieburg.png" + boundary 0 +) + +zone +( + color "green" + pose [ -7.000 -7.000 0 0 ] + name "source" + ctrl "source" +) + +zone +( + color "red" + pose [ 7.000 7.000 0 0 ] + name "sink" + ctrl "sink" +) + +define autorob pioneer2dx +( + sicklaser( samples 32 range_max 5 laser_return 2 ) + ctrl "fasr" +) + +autorob( pose [5.099 4.804 0 -73.937] ) +autorob( pose [6.471 5.304 0 14.941] ) +autorob( pose [5.937 4.858 0 -147.503] ) +autorob( pose [7.574 6.269 0 -111.715] ) +autorob( pose [5.664 6.262 0 -51.799] ) +autorob( pose [7.016 6.428 0 -128.279] ) +autorob( pose [5.911 4.040 0 -97.047] ) +autorob( pose [4.909 6.097 0 -44.366] ) +autorob( pose [6.898 4.775 0 -117.576] ) +autorob( pose [7.401 5.544 0 129.497] ) + +autorob( pose [5.968 7.638 0 170.743] ) +autorob( pose [6.451 4.189 0 -61.453] ) +autorob( pose [5.260 7.436 0 -61.295] ) +autorob( pose [4.149 5.135 0 -90.417] ) +autorob( pose [4.999 4.230 0 -42.157] ) +autorob( pose [4.331 4.217 0 -95.000] ) +autorob( pose [5.440 5.317 0 -26.545] ) +autorob( pose [7.518 6.973 0 163.239] ) +autorob( pose [7.559 4.764 0 -139.066] ) +autorob( pose [4.839 3.595 0 -179.567] ) + +autorob( pose [7.122 4.175 0 -31.440] ) +autorob( pose [5.912 6.963 0 2.937] ) +autorob( pose [6.800 5.897 0 -103.060] ) +autorob( pose [6.331 6.450 0 -103.060] ) +autorob( pose [5.974 5.725 0 -103.060] ) +autorob( pose [4.151 7.272 0 53.540] ) +autorob( pose [6.545 7.459 0 2.937] ) +autorob( pose [7.225 7.459 0 34.450] ) +autorob( pose [3.875 6.533 0 134.717] ) +autorob( pose [3.199 6.081 0 -103.060] ) +autorob( pose [4.634 6.897 0 -103.060] ) + + +autorob( pose [1.288 4.559 0 -103.060] ) +autorob( pose [2.648 5.531 0 -103.060] ) +autorob( pose [2.940 4.559 0 -103.060] ) +autorob( pose [2.228 6.502 0 -103.060] ) +autorob( pose [2.907 7.927 0 -103.060] ) +autorob( pose [0.965 5.595 0 -103.060] ) +autorob( pose [3.847 3.620 0 -103.060] ) +autorob( pose [2.001 4.041 0 -103.060] ) +autorob( pose [2.163 7.603 0 -103.060] ) +autorob( pose [3.393 7.571 0 -103.060] ) + +autorob( pose [1.256 6.373 0 -103.060] ) +autorob( pose [1.062 7.603 0 -103.060] ) +autorob( pose [3.005 3.847 0 -103.060] ) +autorob( pose [1.807 5.272 0 -103.060] ) +autorob( pose [0.317 4.495 0 -103.060] ) +autorob( pose [0.803 3.361 0 -103.060] ) +autorob( pose [1.968 3.102 0 -103.060] ) +autorob( pose [3.976 8.445 0 -103.060] ) +autorob( pose [3.005 8.898 0 -103.060] ) +autorob( pose [1.871 8.445 0 -103.060] ) + +autorob( pose [4.656 7.959 0 -103.060] ) +autorob( pose [0.641 8.574 0 -103.060] ) +autorob( pose [5.012 9.740 0 -103.060] ) +autorob( pose [3.749 9.384 0 -103.060] ) +autorob( pose [3.069 2.681 0 -103.060] ) +autorob( pose [2.260 2.390 0 -103.060] ) +autorob( pose [3.523 1.580 0 -103.060] ) +autorob( pose [3.069 6.858 0 -103.060] ) +autorob( pose [5.142 8.995 0 -103.060] ) +autorob( pose [0.903 6.972 0 -103.060] ) + +autorob( pose [5.405 8.070 0 -103.060] ) +autorob( pose [6.038 8.647 0 -103.060] ) +autorob( pose [2.423 8.379 0 -103.060] ) +autorob( pose [2.437 7.071 0 -103.060] ) +autorob( pose [1.649 6.972 0 -103.060] ) +autorob( pose [4.266 3.019 0 -103.060] ) +autorob( pose [5.490 3.666 0 -103.060] ) +autorob( pose [5.293 2.963 0 -103.060] ) +autorob( pose [3.393 5.228 0 -103.060] ) +autorob( pose [6.418 3.483 0 -103.060] ) + +autorob( pose [6.587 8.464 0 -103.060] ) +autorob( pose [7.178 8.295 0 -103.060] ) +autorob( pose [5.926 2.878 0 -103.060] ) +autorob( pose [7.150 3.568 0 -103.060] ) +autorob( pose [7.783 4.159 0 -103.060] ) +autorob( pose [0.566 6.002 0 -103.060] ) +autorob( pose [1.804 5.847 0 -103.060] ) +autorob( pose [2.423 4.792 0 -103.060] ) +autorob( pose [3.633 4.342 0 -103.060] ) +autorob( pose [0.298 5.129 0 -103.060] ) + + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-10 23:14:58
|
Revision: 7200 http://playerstage.svn.sourceforge.net/playerstage/?rev=7200&view=rev Author: rtv Date: 2008-12-10 23:14:55 +0000 (Wed, 10 Dec 2008) Log Message: ----------- attempting to fix cmake on mac and linux Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/docsrc/stage.dox code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/asr.world Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/CMakeLists.txt 2008-12-10 23:14:55 UTC (rev 7200) @@ -103,8 +103,16 @@ EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images --ldstaticflags OUTPUT_VARIABLE FLTK_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE ) +EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images --libs + OUTPUT_VARIABLE FLTK_LIBS_NEWLINES + OUTPUT_STRIP_TRAILING_WHITESPACE ) +# replace the newlines with semicolons +STRING(REGEX REPLACE "\n" ";" FLTK_LIBS "${FLTK_LIBS_NEWLINES}") + MESSAGE( " FLTK_CFLAGS = ${FLTK_CFLAGS}") MESSAGE( " FLTK_LDFLAGS = ${FLTK_LDFLAGS}") +MESSAGE( " FLTK_LIBS = ${FLTK_LIBS}") + SET (FLTK_FOUND TRUE) #MESSAGE( ${INDENT} "Checking for OpenGL" ) Modified: code/stage/trunk/docsrc/stage.dox =================================================================== --- code/stage/trunk/docsrc/stage.dox 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/docsrc/stage.dox 2008-12-10 23:14:55 UTC (rev 7200) @@ -1086,21 +1086,21 @@ # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) -HAVE_DOT = NO +HAVE_DOT = YES # If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen # will generate a graph for each documented class showing the direct and # indirect inheritance relations. Setting this tag to YES will force the # the CLASS_DIAGRAMS tag to NO. -CLASS_GRAPH = NO +CLASS_GRAPH = YES # If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen # will generate a graph for each documented class showing the direct and # indirect implementation dependencies (inheritance, containment, and # class references variables) of the class with other documented classes. -COLLABORATION_GRAPH = NO +COLLABORATION_GRAPH = YES # If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen # will generate a graph for groups, showing the direct groups dependencies @@ -1138,7 +1138,7 @@ # So in most cases it will be better to enable call graphs for selected # functions only using the \callgraph command. -CALL_GRAPH = NO +CALL_GRAPH = YES # If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen # will graphical hierarchy of all classes instead of a textual one. Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-12-10 23:14:55 UTC (rev 7200) @@ -1,5 +1,8 @@ MESSAGE( STATUS "Configuring libstage" ) +# for config.h +include_directories(${PROJECT_BINARY_DIR}) + set( stageSrcs ancestor.cc block.cc blockgroup.cc @@ -38,22 +41,15 @@ add_library(stage SHARED ${stageSrcs}) +set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) + target_link_libraries( stage ${GLIB_LIBRARIES} ${OPENGL_LIBRARIES} ${LTDL_LIB} + ${FLTK_LIBS2} ) -# When linking stage, pass LDFLAGS from fltk-config -# use quotes to prevent spaces being parsed as a list of properties -set_target_properties( stage PROPERTIES LINK_FLAGS "${FLTK_LDFLAGS}" ) - -# When compiling stageSrcs, pass CFLAGS from fltk-config -set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) - -# for the config.h -include_directories(${PROJECT_BINARY_DIR}) - # causes the shared library to have a version number set_target_properties( stage PROPERTIES VERSION ${VERSION} @@ -67,10 +63,7 @@ # 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 - LINK_FLAGS "${FLTK_LDFLAGS}" -) +set_target_properties( stagebinary PROPERTIES OUTPUT_NAME stage ) target_link_libraries( stagebinary stage ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/libstage/model.cc 2008-12-10 23:14:55 UTC (rev 7200) @@ -419,25 +419,6 @@ g_list_free( list ); } -// // convert a global pose into the model's local coordinate system -// void StgModel::GlobalToLocal( stg_pose_t* pose ) -// { -// // get model's global pose -// stg_pose_t org = GetGlobalPose(); - -// //printf( "g2l global origin %.2f %.2f %.2f\n", -// // org.x, org.y, org.a ); - -// // compute global pose in local coords -// double sx = (pose->x - org.x) * cos(org.a) + (pose->y - org.y) * sin(org.a); -// double sy = -(pose->x - org.x) * sin(org.a) + (pose->y - org.y) * cos(org.a); -// double sa = pose->a - org.a; - -// pose->x = sx; -// pose->y = sy; -// pose->a = sa; -// } - // convert a global pose into the model's local coordinate system stg_pose_t StgModel::GlobalToLocal( stg_pose_t pose ) { @@ -562,9 +543,6 @@ } -// convert a pose in this model's local coordinates into global -// coordinates -// should one day do all this with affine transforms for neatness? inline stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose ) { return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/libstage/model_laser.cc 2008-12-10 23:14:55 UTC (rev 7200) @@ -164,10 +164,7 @@ { // Ignore the model that's looking and things that are invisible to // lasers - if( (hit != finder) && (hit->GetLaserReturn() > 0 ) ) - return true; // match! - - return false; // no match + return( (hit != finder) && (hit->GetLaserReturn() > 0 ) ); } void StgModelLaser::Update( void ) Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/libstage/world.cc 2008-12-10 23:14:55 UTC (rev 7200) @@ -521,8 +521,6 @@ inline int32_t SUPERREGION( const int32_t x ) { - if( abs((x>>SRBITS) > 100 ) ) - printf( "MACRO[ %d %d ]", x, (x>>SRBITS) ); return( x >> SRBITS ); } Modified: code/stage/trunk/worlds/asr.world =================================================================== --- code/stage/trunk/worlds/asr.world 2008-12-10 18:00:48 UTC (rev 7199) +++ code/stage/trunk/worlds/asr.world 2008-12-10 23:14:55 UTC (rev 7200) @@ -41,7 +41,7 @@ ( name "cave" size [100.000 100.000 0.600] - pose [0 0 0 0] + pose [0 0 0 50] bitmap "bitmaps/frieburg.png" boundary 0 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-11 01:10:39
|
Revision: 7201 http://playerstage.svn.sourceforge.net/playerstage/?rev=7201&view=rev Author: rtv Date: 2008-12-11 01:10:35 +0000 (Thu, 11 Dec 2008) Log Message: ----------- tweaked view dialog behaviour and text Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/worlds/asr.world Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-12-10 23:14:55 UTC (rev 7200) +++ code/stage/trunk/libstage/canvas.cc 2008-12-11 01:10:35 UTC (rev 7201) @@ -74,7 +74,7 @@ showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false ), showBlur( "Trails/Blur", "show_trailblur", "^d", false ), pCamOn( "Perspective camera", "pcam_on", "r", false ), - visualizeAll( "Visualize All", "vis_all", "^v", true ), + visualizeAll( "Selected only", "vis_all", "^v", false ), // and the rest graphics( true ), world( world ) @@ -795,7 +795,7 @@ //LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); // MOTION BLUR - if( showBlur ) + if( 0 )//showBlur ) { DrawBlocks(); @@ -819,7 +819,7 @@ } // GRAY TRAILS -// if( showBlocks ) +// if( showBlur ) // { // static float count = 0; @@ -846,7 +846,7 @@ // } // PRETTY BLACK -// if( showBlocks ) +// if( showBlur && showBlocks ) // { // static float count = 0; @@ -900,7 +900,7 @@ // draw the model-specific visualizations if( showData ) { - if ( visualizeAll ) { + if ( ! visualizeAll ) { for( GList* it = world->StgWorld::children; it; it=it->next ) ((StgModel*)it->data)->DataVisualizeTree( current_camera ); } @@ -1051,7 +1051,6 @@ png_set_rows( pp, info, rowpointers ); - //png_set_compression_level(pp, Z_DEFAULT_COMPRESSION); png_set_IHDR( pp, info, width, height, 8, PNG_COLOR_TYPE_RGBA, @@ -1061,7 +1060,7 @@ png_write_png( pp, info, PNG_TRANSFORM_IDENTITY, NULL ); - // free the PNG data - we reuse the pixel data next time. + // free the PNG data - we reuse the pixel array next call. png_destroy_write_struct(&pp, &info); fclose(fp); Modified: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc 2008-12-10 23:14:55 UTC (rev 7200) +++ code/stage/trunk/libstage/options_dlg.cc 2008-12-11 01:10:35 UTC (rev 7201) @@ -4,12 +4,14 @@ namespace Stg { OptionsDlg::OptionsDlg( int x, int y, int w, int h ) : - Fl_Window( x,y, w,h, "Filter" ), + Fl_Window( x,y, w,h, "Visualize" ), changedItem( NULL ), showAll( NULL ), status( NO_EVENT ), hm( w/6 ) { + set_non_modal(); // keep on top but do not grab all events + showAllCheck = new Fl_Check_Button( 0,0, w,boxH ); showAllCheck->callback( checkChanged, this ); @@ -34,21 +36,23 @@ Fl_Check_Button* check = static_cast<Fl_Check_Button*>( w ); OptionsDlg* oDlg = static_cast<OptionsDlg*>( p ); - if ( check == oDlg->showAllCheck && oDlg->showAll ) { - oDlg->status = CHANGE_ALL; - oDlg->showAll->set( check->value() ); - oDlg->do_callback(); - oDlg->status = NO_EVENT; - } - else { - int item = oDlg->scroll->find( check ); - oDlg->options[ item ]->set( check->value() ); - oDlg->changedItem = oDlg->options[ item ]; - oDlg->status = CHANGE; - oDlg->do_callback(); - oDlg->changedItem = NULL; - oDlg->status = NO_EVENT; - } + if ( check == oDlg->showAllCheck && oDlg->showAll ) + { + oDlg->status = CHANGE_ALL; + oDlg->showAll->set( check->value() ); + oDlg->do_callback(); + oDlg->status = NO_EVENT; + } + else + { + int item = oDlg->scroll->find( check ); + oDlg->options[ item ]->set( check->value() ); + oDlg->changedItem = oDlg->options[ item ]; + oDlg->status = CHANGE; + oDlg->do_callback(); + oDlg->changedItem = NULL; + oDlg->status = NO_EVENT; + } } void OptionsDlg::closePress( Fl_Widget* w, void* p ) { @@ -59,13 +63,9 @@ oDlg->status = NO_EVENT; } - int OptionsDlg::handle( int event ) { - // switch ( event ) { - // - // } - - return Fl_Window::handle( event ); - } +// int OptionsDlg::handle( int event ) { +// return Fl_Window::handle( event ); +// } void OptionsDlg::updateChecks() { Modified: code/stage/trunk/libstage/options_dlg.hh =================================================================== --- code/stage/trunk/libstage/options_dlg.hh 2008-12-10 23:14:55 UTC (rev 7200) +++ code/stage/trunk/libstage/options_dlg.hh 2008-12-11 01:10:35 UTC (rev 7201) @@ -30,7 +30,7 @@ Fl_Check_Button* showAllCheck; void updateChecks(); - virtual int handle( int event ); + //virtual int handle( int event ); static void checkChanged( Fl_Widget* w, void* p ); static void closePress( Fl_Widget* w, void* p ); Modified: code/stage/trunk/worlds/asr.world =================================================================== --- code/stage/trunk/worlds/asr.world 2008-12-10 23:14:55 UTC (rev 7200) +++ code/stage/trunk/worlds/asr.world 2008-12-11 01:10:35 UTC (rev 7201) @@ -20,10 +20,10 @@ # configure the GUI window window ( - size [ 1916.000 1156.000 ] + size [ 967.000 744.000 ] - center [ 1.814 5.214 ] - rotate [ 0 0 ] + center [ 3.759 6.706 ] + rotate [ 70.000 5.000 ] scale 71.081 pcam_loc [ 0 -4.000 2.000 ] @@ -41,7 +41,7 @@ ( name "cave" size [100.000 100.000 0.600] - pose [0 0 0 50] + pose [0 0 0 50.000] bitmap "bitmaps/frieburg.png" boundary 0 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-03 00:04:02
|
Revision: 7224 http://playerstage.svn.sourceforge.net/playerstage/?rev=7224&view=rev Author: rtv Date: 2009-01-03 00:03:56 +0000 (Sat, 03 Jan 2009) Log Message: ----------- cleaning up and documenting for 3.1 release Modified Paths: -------------- code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/examples/ctrl/lasernoise.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/asr.world code/stage/trunk/worlds/fasr.world Added Paths: ----------- code/stage/trunk/libstage/model.hh Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-03 00:03:56 UTC (rev 7224) @@ -5,6 +5,7 @@ sink source wander + trail ) Modified: code/stage/trunk/examples/ctrl/lasernoise.cc =================================================================== --- code/stage/trunk/examples/ctrl/lasernoise.cc 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/examples/ctrl/lasernoise.cc 2009-01-03 00:03:56 UTC (rev 7224) @@ -10,7 +10,7 @@ const double DEVIATION = 0.05; -double SimpleNormalDeviate( double mean, double stddev ) +double simple_normal_deviate( double mean, double stddev ) { double x = 0.0; @@ -29,7 +29,7 @@ if( scan ) for( unsigned int i=0; i<sample_count; i++ ) - scan[i].range *= SimpleNormalDeviate( 1.0, DEVIATION ); + scan[i].range *= simple_normal_deviate( 1.0, DEVIATION ); return 0; // run again } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/libstage/canvas.cc 2009-01-03 00:03:56 UTC (rev 7224) @@ -77,7 +77,9 @@ visualizeAll( "Selected only", "vis_all", "^v", false ), // and the rest graphics( true ), - world( world ) + world( world ), + frames_rendered_count( 0 ), + screenshot_frame_skip( 1 ) { end(); @@ -1000,9 +1002,10 @@ glMatrixMode (GL_MODELVIEW); } - - if( showScreenshots ) + if( showScreenshots && (frames_rendered_count % screenshot_frame_skip == 0) ) Screenshot(); + + frames_rendered_count++; } @@ -1115,7 +1118,11 @@ perspective_camera.Load( wf, sec ); interval = wf->ReadInt(sec, "interval", interval ); - + + screenshot_frame_skip = wf->ReadInt( sec, "screenshot_skip", screenshot_frame_skip ); + if( screenshot_frame_skip < 1 ) + screenshot_frame_skip = 1; // avoids div-by-zero if poorly set + showData.Load( wf, sec ); showFlags.Load( wf, sec ); showBlocks.Load( wf, sec ); Added: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh (rev 0) +++ code/stage/trunk/libstage/model.hh 2009-01-03 00:03:56 UTC (rev 7224) @@ -0,0 +1,1126 @@ +#ifndef MODEL_H +#define MODEL_H + +/// %StgModel class +class StgModel : public StgAncestor +{ + friend class StgAncestor; + friend class StgWorld; + friend class StgWorldGui; + friend class StgCanvas; + friend class StgBlock; + friend class Region; + friend class BlockGroup; + +private: + /** the number of models instatiated - used to assign unique IDs */ + static uint32_t count; + static GHashTable* modelsbyid; + std::vector<Option*> drawOptions; + const std::vector<Option*>& getOptions() const { return drawOptions; } + +protected: + //static const char* typestr; + + GMutex* access_mutex; + GPtrArray* blinkenlights; + bool blob_return; + BlockGroup blockgroup; + /** OpenGL display list identifier for the blockgroup */ + int blocks_dl; + + int boundary; + + /** callback functions can be attached to any field in this + structure. When the internal function model_change(<mod>,<field>) + is called, the callback is triggered */ + GHashTable* callbacks; + + stg_color_t color; + /** This can be set to indicate that the model has new data that + may be of interest to users. This allows polling the model + instead of adding a data callback. */ + bool data_fresh; + stg_bool_t disabled; //< if non-zero, the model is disabled + int fiducial_key; + int fiducial_return; + GList* flag_list; + stg_geom_t geom; + stg_pose_t global_pose; + bool gpose_dirty; //< set this to indicate that global pose may have changed + bool gripper_return; + int gui_grid; + int gui_mask; + int gui_nose; + int gui_outline; + bool has_default_block; + + /* hooks for attaching special callback functions (not used as + variables) */ + char hook_load; + char hook_save; + char hook_shutdown; + char hook_startup; + char hook_update; + + /** unique process-wide identifier for this model */ + uint32_t id; + ctrlinit_t* initfunc; + stg_usec_t interval; //< time between updates in us + stg_laser_return_t laser_return; + stg_usec_t last_update; //< time of last update in us + bool map_caches_are_invalid; + stg_meters_t map_resolution; + stg_kg_t mass; + bool obstacle_return; + bool on_update_list; + bool on_velocity_list; + StgModel* parent; //< the model that owns this one, possibly NUL + stg_pose_t pose; + /** GData datalist can contain arbitrary named data items. Can be used + by derived model types to store properties, and for user code + to associate arbitrary items with a model. */ + GData* props; + bool ranger_return; + bool rebuild_displaylist; //< iff true, regenerate block display list before redraw + char* say_string; //< if non-null, this string is displayed in the GUI + + stg_bool_t stall; + /** Thread safety flag. Iff true, Update() may be called in + parallel with other models. Defaults to false for safety */ + int subs; //< the number of subscriptions to this model + bool thread_safe; + GArray* trail; + stg_model_type_t type; + bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() + stg_velocity_t velocity; + stg_watts_t watts; //< power consumed by this model + Worldfile* wf; + int wf_entity; + StgWorld* world; // pointer to the world in which this model exists + +public: + void Lock() + { + if( access_mutex == NULL ) + access_mutex = g_mutex_new(); + + assert( access_mutex ); + g_mutex_lock( access_mutex ); + } + + void Unlock() + { + assert( access_mutex ); + g_mutex_unlock( access_mutex ); + } + + +private: + /** Private copy constructor declared but not defined, to make it + impossible to copy models. */ + explicit StgModel(const StgModel& original); + + /** Private assignment operator declared but not defined, to make it + impossible to copy models */ + StgModel& operator=(const StgModel& original); + +protected: + + /// Register an Option for pickup by the GUI + void registerOption( Option* opt ) + { drawOptions.push_back( opt ); } + + /** Check to see if the current pose will yield a collision with + obstacles. Returns a pointer to the first entity we are in + collision with, or NULL if no collision exists. */ + StgModel* TestCollision(); + + void CommitTestedPose(); + + void Map(); + void UnMap(); + + void MapWithChildren(); + void UnMapWithChildren(); + + int TreeToPtrArray( GPtrArray* array ); + + /** raytraces a single ray from the point and heading identified by + pose, in local coords */ + stg_raytrace_result_t Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest = true ); + + /** raytraces multiple rays around the point and heading identified + by pose, in local coords */ + void Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest = true ); + + stg_raytrace_result_t Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest = true ); + + void Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest = true ); + + + /** Causes this model and its children to recompute their global + position instead of using a cached pose in + StgModel::GetGlobalPose()..*/ + void GPoseDirtyTree(); + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void UpdatePose(); + + void StartUpdating(); + void StopUpdating(); + + StgModel* ConditionalMove( stg_pose_t newpose ); + + stg_meters_t ModelHeight(); + + bool UpdateDue( void ); + void UpdateIfDue(); + + void DrawBlocksTree(); + virtual void DrawBlocks(); + void DrawBoundingBox(); + void DrawBoundingBoxTree(); + virtual void DrawStatus( Camera* cam ); + void DrawStatusTree( Camera* cam ); + + void DrawOriginTree(); + void DrawOrigin(); + + void PushLocalCoords(); + void PopCoords(); + + ///Draw the image stored in texture_id above the model + void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); + + + // static wrapper for DrawBlocks() + static void DrawBlocks( gpointer dummykey, + StgModel* mod, + void* arg ); + + virtual void DrawPicker(); + virtual void DataVisualize( Camera* cam ); + + virtual void DrawSelected(void); + + void DrawTrailFootprint(); + void DrawTrailBlocks(); + void DrawTrailArrows(); + void DrawGrid(); + + + void DrawBlinkenlights(); + + void DataVisualizeTree( Camera* cam ); + + virtual void PushColor( stg_color_t col ) + { world->PushColor( col ); } + + virtual void PushColor( double r, double g, double b, double a ) + { world->PushColor( r,g,b,a ); } + + virtual void PopColor(){ world->PopColor(); } + + void DrawFlagList(); + + void DrawPose( stg_pose_t pose ); + +public: + void RecordRenderPoint( GSList** head, GSList* link, + unsigned int* c1, unsigned int* c2 ); + + void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ); + + /** Look up a model pointer by a unique model ID */ + static StgModel* LookupId( uint32_t id ) + { return (StgModel*)g_hash_table_lookup( modelsbyid, (void*)id ); } + + /** Constructor */ + StgModel( StgWorld* world, + StgModel* parent, + stg_model_type_t type = MODEL_TYPE_PLAIN ); + + /** Destructor */ + virtual ~StgModel(); + + void Say( const char* str ); + + void Load( Worldfile* wf, int wf_entity ) + { + /** Set the worldfile and worldfile entity ID - must be called + before Load() */ + SetWorldfile( wf, wf_entity ); + Load(); // call virtual load + } + + /** Set the worldfile and worldfile entity ID */ + void SetWorldfile( Worldfile* wf, int wf_entity ) + { this->wf = wf; this->wf_entity = wf_entity; } + + /** configure a model by reading from the current world file */ + virtual void Load(); + + /** save the state of the model to the current world file */ + virtual void Save(); + + /** Should be called after all models are loaded, to do any last-minute setup */ + void Init(); + void InitRecursive(); + + void AddFlag( StgFlag* flag ); + void RemoveFlag( StgFlag* flag ); + + void PushFlag( StgFlag* flag ); + StgFlag* PopFlag(); + + int GetFlagCount(){ return g_list_length( flag_list ); } + + /** Add a pointer to a blinkenlight to the model. */ + void AddBlinkenlight( stg_blinkenlight_t* b ) + { g_ptr_array_add( this->blinkenlights, b ); } + + /** Clear all blinkenlights from the model. Does not destroy the + blinkenlight objects. */ + void ClearBlinkenlights() + { g_ptr_array_set_size( this->blinkenlights, 0 ); } + + /** Disable the model. Its pose will not change due to velocity + until re-enabled using Enable(). This is used for example when + dragging a model with the mouse pointer. The model is enabled by + default. */ + void Disable(){ disabled = true; }; + + /** Enable the model, so that non-zero velocities will change the + model's pose. Models are enabled by default. */ + void Enable(){ disabled = false; }; + + /** Load a control program for this model from an external + library */ + void LoadControllerModule( char* lib ); + + /** Sets the redraw flag, so this model will be redrawn at the + earliest opportunity */ + void NeedRedraw(); + + /** Add a block to this model by loading it from a worldfile + entity */ + void LoadBlock( Worldfile* wf, int entity ); + + /** Add a block to this model centered at [x,y] with extent [dx, dy, + dz] */ + void AddBlockRect( stg_meters_t x, stg_meters_t y, + stg_meters_t dx, stg_meters_t dy, + stg_meters_t dz ); + + /** remove all blocks from this model, freeing their memory */ + void ClearBlocks(); + + /** Returns a pointer to this model's parent model, or NULL if this + model has no parent */ + StgModel* Parent(){ return this->parent; } + + StgModel* GetModel( const char* name ); + int GuiMask(){ return this->gui_mask; }; + + /** Returns a pointer to the world that contains this model */ + StgWorld* GetWorld(){ return this->world; } + + /** return the root model of the tree containing this model */ + StgModel* Root(){ return( parent ? parent->Root() : this ); } + + /** Returns this model's visibility to collision detection */ + bool ObstacleReturn(){ return obstacle_return; } + + /** Returns this model's visibilty to laser sensors */ + stg_laser_return_t GetLaserReturn(){ return laser_return; } + + /** Returns true iff the model is detected by a ranger sensor */ + bool GetRangerReturn(){ return ranger_return; } + + /** Returns the value that this model presents to a fiducial + finder sensor */ + int FiducialReturn(){ return fiducial_return; } + + /** Returns the fiducial key, used to distinguish types of + fiducials */ + int FiducialKey(){ return fiducial_key; } + + bool IsAntecedent( StgModel* testmod ); + + /** returns true if model [testmod] is a descendent of this model */ + bool IsDescendent( StgModel* testmod ); + + /** returns true if model [testmod] is a descendent or antecedent of this model */ + bool IsRelated( StgModel* testmod ); + + /** get the pose of a model in the global CS */ + stg_pose_t GetGlobalPose(); + + /** get the velocity of a model in the global CS */ + stg_velocity_t GetGlobalVelocity(); + + /* set the velocity of a model in the global coordinate system */ + void SetGlobalVelocity( stg_velocity_t gvel ); + + /** subscribe to a model's data */ + void Subscribe(); + + /** unsubscribe from a model's data */ + void Unsubscribe(); + + /** set the pose of model in global coordinates */ + void SetGlobalPose( stg_pose_t gpose ); + + /** set a model's velocity in its parent's coordinate system */ + void SetVelocity( stg_velocity_t vel ); + + /** set a model's pose in its parent's coordinate system */ + void SetPose( stg_pose_t pose ); + + /** add values to a model's pose in its parent's coordinate system */ + void AddToPose( stg_pose_t pose ); + + /** add values to a model's pose in its parent's coordinate system */ + void AddToPose( double dx, double dy, double dz, double da ); + + /** set a model's geometry (size and center offsets) */ + void SetGeom( stg_geom_t src ); + + /** set a model's geometry (size and center offsets) */ + void SetFiducialReturn( int fid ); + + /** set a model's fiducial key: only fiducial finders with a + matching key can detect this model as a fiducial. */ + void SetFiducialKey( int key ); + + stg_color_t GetColor(){ return color; } + + // stg_laser_return_t GetLaserReturn(){ return laser_return; } + + /** Change a model's parent - experimental*/ + int SetParent( StgModel* newparent); + + /** Get a model's geometry - it's size and local pose (offset from + origin in local coords) */ + stg_geom_t GetGeom(){ return geom; } + + /** Get the pose of a model in its parent's coordinate system */ + stg_pose_t GetPose(){ return pose; } + + /** Get a model's velocity (in its local reference frame) */ + stg_velocity_t GetVelocity(){ return velocity; } + + // guess what these do? + void SetColor( stg_color_t col ); + void SetMass( stg_kg_t mass ); + void SetStall( stg_bool_t stall ); + void SetGripperReturn( int val ); + void SetLaserReturn( stg_laser_return_t val ); + void SetObstacleReturn( int val ); + void SetBlobReturn( int val ); + void SetRangerReturn( int val ); + void SetBoundary( int val ); + void SetGuiNose( int val ); + void SetGuiMask( int val ); + void SetGuiGrid( int val ); + void SetGuiOutline( int val ); + void SetWatts( stg_watts_t watts ); + void SetMapResolution( stg_meters_t res ); + + bool DataIsFresh(){ return this->data_fresh; } + + /* attach callback functions to data members. The function gets + called when the member is changed using SetX() accessor method */ + + void AddCallback( void* address, + stg_model_callback_t cb, + void* user ); + + int RemoveCallback( void* member, + stg_model_callback_t callback ); + + void CallCallbacks( void* address ); + + /* wrappers for the generic callback add & remove functions that hide + some implementation detail */ + + void AddStartupCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hook_startup, cb, user ); }; + + void RemoveStartupCallback( stg_model_callback_t cb ) + { RemoveCallback( &hook_startup, cb ); }; + + void AddShutdownCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hook_shutdown, cb, user ); }; + + void RemoveShutdownCallback( stg_model_callback_t cb ) + { RemoveCallback( &hook_shutdown, cb ); } + + void AddLoadCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hook_load, cb, user ); } + + void RemoveLoadCallback( stg_model_callback_t cb ) + { RemoveCallback( &hook_load, cb ); } + + void AddSaveCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hook_save, cb, user ); } + + void RemoveSaveCallback( stg_model_callback_t cb ) + { RemoveCallback( &hook_save, cb ); } + + void AddUpdateCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hook_update, cb, user ); } + + void RemoveUpdateCallback( stg_model_callback_t cb ) + { RemoveCallback( &hook_update, cb ); } + + /** named-property interface + */ + void* GetProperty( char* key ); + + /** @brief Set a named property of a Stage model. + + Set a property of a Stage model. + + This function can set both predefined and user-defined + properties of a model. Predefined properties are intrinsic to + every model, such as pose and color. Every supported predefined + properties has its identifying string defined as a preprocessor + macro in stage.h. Users should use the macro instead of a + hard-coded string, so that the compiler can help you to avoid + mis-naming properties. + + User-defined properties allow the user to attach arbitrary data + pointers to a model. User-defined property data is not copied, + so the original pointer must remain valid. User-defined property + names are simple strings. Names beginning with an underscore + ('_') are reserved for internal libstage use: users should not + use names beginning with underscore (at risk of causing very + weird behaviour). + + Any callbacks registered for the named property will be called. + + Returns 0 on success, or a positive error code on failure. + + *CAUTION* The caller is responsible for making sure the pointer + points to data of the correct type for the property, so use + carefully. Check the docs or the equivalent + stg_model_set_<property>() function definition to see the type + of data required for each property. + */ + int SetProperty( char* key, void* data ); + void UnsetProperty( char* key ); + + virtual void Print( char* prefix ); + virtual const char* PrintWithPose(); + + /** Convert a pose in the world coordinate system into a model's + local coordinate system. Overwrites [pose] with the new + coordinate. */ + stg_pose_t GlobalToLocal( const stg_pose_t pose ); + + /** Return the global pose (i.e. pose in world coordinates) of a + pose specified in the model's local coordinate system */ + stg_pose_t LocalToGlobal( const stg_pose_t 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 ); + + /** returns the first descendent of this model that is unsubscribed + and has the type indicated by the string */ + StgModel* GetUnsubscribedModelOfType( const stg_model_type_t type ); + + /** returns the first descendent of this model that is unused + and has the type indicated by the string. This model is tagged as used. */ + StgModel* GetUnusedModelOfType( const stg_model_type_t type ); + + /** Returns the value of the model's stall boolean, which is true + iff the model has crashed into another model */ + bool Stalled(){ return this->stall; } +}; + + + // BLOBFINDER MODEL -------------------------------------------------------- + + /** blobfinder data packet + */ + typedef struct + { + stg_color_t color; + uint32_t left, top, right, bottom; + stg_meters_t range; + } stg_blobfinder_blob_t; + + /// %StgModelBlobfinder class + class StgModelBlobfinder : public StgModel + { + private: + GArray* colors; + GArray* blobs; + + // predicate for ray tracing + static bool BlockMatcher( StgBlock* testblock, StgModel* finder ); + + static Option showBlobData; + + virtual void DataVisualize( Camera* cam ); + + public: + unsigned int scan_width; + unsigned int scan_height; + stg_meters_t range; + stg_radians_t fov; + stg_radians_t pan; + + static const char* typestr; + + // constructor + StgModelBlobfinder( StgWorld* world, + StgModel* parent ); + // destructor + ~StgModelBlobfinder(); + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Load(); + + stg_blobfinder_blob_t* GetBlobs( unsigned int* count ) + { + if( count ) *count = blobs->len; + return (stg_blobfinder_blob_t*)blobs->data; + } + + /** Start finding blobs with this color.*/ + void AddColor( stg_color_t col ); + + /** Stop tracking blobs with this color */ + void RemoveColor( stg_color_t col ); + + /** Stop tracking all colors. Call this to clear the defaults, then + add colors individually with AddColor(); */ + void RemoveAllColors(); + }; + + // ENERGY model -------------------------------------------------------------- + + /** energy data packet */ + typedef struct + { + /** estimate of current energy stored */ + stg_joules_t stored; + + /** TRUE iff the device is receiving energy from a charger */ + stg_bool_t charging; + + /** diatance to charging device */ + stg_meters_t range; + + /** an array of pointers to connected models */ + GPtrArray* connections; + } stg_energy_data_t; + + /** energy config packet (use this to set or get energy configuration)*/ + typedef struct + { + /** maximum storage capacity */ + stg_joules_t capacity; + + /** When charging another device, supply this many Joules/sec at most*/ + stg_watts_t give_rate; + + /** When charging from another device, receive this many Joules/sec at most*/ + stg_watts_t take_rate; + + /** length of the charging probe */ + stg_meters_t probe_range; + + /** iff TRUE, this device will supply power to connected devices */ + stg_bool_t give; + + } stg_energy_config_t; + + // there is currently no energy command packet + + + // LASER MODEL -------------------------------------------------------- + + /** laser sample packet + */ + typedef struct + { + stg_meters_t range; ///< range to laser hit in meters + double reflectance; ///< intensity of the reflection 0.0 to 1.0 + } stg_laser_sample_t; + + typedef struct + { + uint32_t sample_count; + uint32_t resolution; + stg_bounds_t range_bounds; + stg_radians_t fov; + stg_usec_t interval; + } stg_laser_cfg_t; + + /// %StgModelLaser class + class StgModelLaser : public StgModel + { + private: + /** OpenGL displaylist for laser data */ + int data_dl; + bool data_dirty; + + stg_laser_sample_t* samples; + uint32_t sample_count; + stg_meters_t range_min, range_max; + stg_radians_t fov; + uint32_t resolution; + + static Option showLaserData; + static Option showLaserStrikes; + + public: + static const char* typestr; + // constructor + StgModelLaser( StgWorld* world, + StgModel* parent ); + + // destructor + ~StgModelLaser(); + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Load(); + virtual void Print( char* prefix ); + virtual void DataVisualize( Camera* cam ); + + uint32_t GetSampleCount(){ return sample_count; } + + stg_laser_sample_t* GetSamples( uint32_t* count=NULL); + + void SetSamples( stg_laser_sample_t* samples, uint32_t count); + + // Get the user-tweakable configuration of the laser + stg_laser_cfg_t GetConfig( ); + + // Set the user-tweakable configuration of the laser + void SetConfig( stg_laser_cfg_t cfg ); + }; + + // \todo GRIPPER MODEL -------------------------------------------------------- + + // typedef enum { + // STG_GRIPPER_PADDLE_OPEN = 0, // default state + // STG_GRIPPER_PADDLE_CLOSED, + // STG_GRIPPER_PADDLE_OPENING, + // STG_GRIPPER_PADDLE_CLOSING, + // } stg_gripper_paddle_state_t; + + // typedef enum { + // STG_GRIPPER_LIFT_DOWN = 0, // default state + // STG_GRIPPER_LIFT_UP, + // STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state + // STG_GRIPPER_LIFT_DOWNING, + // } stg_gripper_lift_state_t; + + // typedef enum { + // STG_GRIPPER_CMD_NOP = 0, // default state + // STG_GRIPPER_CMD_OPEN, + // STG_GRIPPER_CMD_CLOSE, + // STG_GRIPPER_CMD_UP, + // STG_GRIPPER_CMD_DOWN + // } stg_gripper_cmd_type_t; + + // /** gripper configuration packet + // */ + // typedef struct + // { + // stg_size_t paddle_size; ///< paddle dimensions + + // stg_gripper_paddle_state_t paddles; + // stg_gripper_lift_state_t lift; + + // double paddle_position; ///< 0.0 = full open, 1.0 full closed + // double lift_position; ///< 0.0 = full down, 1.0 full up + + // stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle + // stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle + // stg_bool_t paddles_stalled; // true iff some solid object stopped + // // the paddles closing or opening + + // GSList *grip_stack; ///< stack of items gripped + // int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited + + // double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. + + // } stg_gripper_config_t; + + // /** gripper command packet + // */ + // typedef struct + // { + // stg_gripper_cmd_type_t cmd; + // int arg; + // } stg_gripper_cmd_t; + + + // /** gripper data packet + // */ + // typedef struct + // { + // stg_gripper_paddle_state_t paddles; + // stg_gripper_lift_state_t lift; + + // double paddle_position; ///< 0.0 = full open, 1.0 full closed + // double lift_position; ///< 0.0 = full down, 1.0 full up + + // stg_bool_t inner_break_beam; ///< non-zero iff beam is broken + // stg_bool_t outer_break_beam; ///< non-zero iff beam is broken + + // stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something + + // stg_bool_t paddles_stalled; // true iff some solid object stopped + // // the paddles closing or opening + + // int stack_count; ///< number of objects in stack + + + // } stg_gripper_data_t; + + + // \todo BUMPER MODEL -------------------------------------------------------- + + // typedef struct + // { + // stg_pose_t pose; + // stg_meters_t length; + // } stg_bumper_config_t; + + // typedef struct + // { + // StgModel* hit; + // stg_point_t hit_point; + // } stg_bumper_sample_t; + + + // 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 + stg_pose_t geom; ///< size and relative angle of the target + stg_pose_t pose; ///< Absolute accurate position of the target in world coordinates (it's cheating to use this in robot controllers!) + int id; ///< the identifier of the target, or -1 if none can be detected. + + } stg_fiducial_t; + + /// %StgModelFiducial class + class StgModelFiducial : public StgModel + { + private: + // if neighbor is visible, add him to the fiducial scan + void AddModelIfVisible( StgModel* him ); + + // static wrapper function can be used as a function pointer + static int AddModelIfVisibleStatic( StgModel* him, StgModelFiducial* me ) + { if( him != me ) me->AddModelIfVisible( him ); return 0; } + + virtual void Update(); + virtual void DataVisualize( Camera* cam ); + + GArray* data; + + static Option showFiducialData; + + public: + static const char* typestr; + // constructor + StgModelFiducial( StgWorld* world, + StgModel* parent ); + // destructor + virtual ~StgModelFiducial(); + + 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 + }; + + + // RANGER MODEL -------------------------------------------------------- + + typedef struct + { + stg_pose_t pose; + stg_size_t size; + stg_bounds_t bounds_range; + stg_radians_t fov; + int ray_count; + } stg_ranger_sensor_t; + + /// %StgModelRanger class + class StgModelRanger : public StgModel + { + protected: + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void DataVisualize( Camera* cam ); + + public: + static const char* typestr; + // constructor + StgModelRanger( StgWorld* world, + StgModel* parent ); + // destructor + virtual ~StgModelRanger(); + + virtual void Load(); + virtual void Print( char* prefix ); + + uint32_t sensor_count; + stg_ranger_sensor_t* sensors; + stg_meters_t* samples; + + private: + static Option showRangerData; + static Option showRangerTransducers; + + }; + + // BLINKENLIGHT MODEL ---------------------------------------------------- + class StgModelBlinkenlight : public StgModel + { + private: + double dutycycle; + bool enabled; + stg_msec_t period; + bool on; + + static Option showBlinkenData; + public: + + static const char* typestr; + StgModelBlinkenlight( StgWorld* world, + StgModel* parent ); + + ~StgModelBlinkenlight(); + + virtual void Load(); + virtual void Update(); + virtual void DataVisualize( Camera* cam ); + }; + + // CAMERA MODEL ---------------------------------------------------- + typedef struct { + // GL_V3F + GLfloat x, y, z; + } ColoredVertex; + + /// %StgModelCamera class + class StgModelCamera : public StgModel + { + private: + StgCanvas* _canvas; + + GLfloat* _frame_data; //opengl read buffer + GLubyte* _frame_color_data; //opengl read buffer + + bool _valid_vertexbuf_cache; + ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length) + + int _width; //width of buffer + int _height; //height of buffer + static const int _depth = 4; + + int _camera_quads_size; + GLfloat* _camera_quads; + GLubyte* _camera_colors; + + static Option showCameraData; + + PerspectiveCamera _camera; + float _yaw_offset; //position camera is mounted at + float _pitch_offset; + + ///Take a screenshot from the camera's perspective. return: true for sucess, and data is available via FrameDepth() / FrameColor() + bool GetFrame(); + + public: + StgModelCamera( StgWorld* world, + StgModel* parent ); + + ~StgModelCamera(); + + virtual void Load(); + + ///Capture a new frame ( calls GetFrame ) + virtual void Update(); + + ///Draw Camera Model - TODO + //virtual void Draw( uint32_t flags, StgCanvas* canvas ); + + ///Draw camera visualization + virtual void DataVisualize( Camera* cam ); + + ///width of captured image + inline int getWidth( void ) const { return _width; } + + ///height of captured image + inline int getHeight( void ) const { return _height; } + + ///get reference to camera used + inline const PerspectiveCamera& getCamera( void ) const { return _camera; } + + ///get a reference to camera depth buffer + inline const GLfloat* FrameDepth() const { return _frame_data; } + + ///get a reference to camera color image. 3 bytes (RGB) per pixel + inline const GLubyte* FrameColor() const { return _frame_color_data; } + + ///change the pitch + inline void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; } + + ///change the yaw + inline void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; } + }; + + // POSITION MODEL -------------------------------------------------------- + + /** Define a position control method */ + typedef enum + { STG_POSITION_CONTROL_VELOCITY, + STG_POSITION_CONTROL_POSITION + } stg_position_control_mode_t; + + /** Define a localization method */ + typedef enum + { STG_POSITION_LOCALIZATION_GPS, + STG_POSITION_LOCALIZATION_ODOM + } stg_position_localization_mode_t; + + /** Define a driving method */ + typedef enum + { STG_POSITION_DRIVE_DIFFERENTIAL, + STG_POSITION_DRIVE_OMNI, + STG_POSITION_DRIVE_CAR + } stg_position_drive_mode_t; + + + /// %StgModelPosition class + class StgModelPosition : public StgModel + { + friend class StgCanvas; + + private: + stg_pose_t goal; //< the current velocity or pose to reach, depending on the value of control_mode + stg_position_control_mode_t control_mode; + stg_position_drive_mode_t drive_mode; + stg_position_localization_mode_t localization_mode; ///< global or local mode + stg_velocity_t integration_error; ///< errors to apply in simple odometry model + + Waypoint* waypoints; + uint32_t waypoint_count; + void DrawWaypoints(); + + private: + static Option showCoords; + static Option showWaypoints; + + public: + static const char* typestr; + // constructor + StgModelPosition( StgWorld* world, + StgModel* parent ); + // destructor + ~StgModelPosition(); + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Load(); + + virtual void DataVisualize( Camera* cam ); + + /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ + Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); + + /** Set the current pose estimate.*/ + void SetOdom( stg_pose_t odom ); + + /** Sets the control_mode to STG_POSITION_CONTROL_VELOCITY and sets + the goal velocity. */ + void SetSpeed( double x, double y, double a ); + void SetXSpeed( double x ); + void SetYSpeed( double y ); + void SetZSpeed( double z ); + void SetTurnSpeed( double a ); + void SetSpeed( stg_velocity_t vel ); + + /** Sets the control mode to STG_POSITION_CONTROL_POSITION and sets + the goal pose */ + void GoTo( double x, double y, double a ); + void GoTo( stg_pose_t pose ); + + // localization state + stg_pose_t est_pose; ///< position estimate in local coordinates + stg_pose_t est_pose_error; ///< estimated error in position estimate + stg_pose_t est_origin; ///< global origin of the local coordinate system + }; + +#endif // MODEL_H Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/libstage/model_laser.cc 2009-01-03 00:03:56 UTC (rev 7224) @@ -129,6 +129,9 @@ range_max = wf->ReadLength( wf_entity, "range_max", range_max ); fov = wf->ReadAngle( wf_entity, "fov", fov ); resolution = wf->ReadInt( wf_entity, "resolution", resolution ); + + showLaserData.Load( wf, wf_entity ); + showLaserStrikes.Load( wf, wf_entity ); if( resolution < 1 ) { Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/libstage/model_load.cc 2009-01-03 00:03:56 UTC (rev 7224) @@ -191,7 +191,7 @@ char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL ); if( !lib ) - puts( "Error - NULL library name" ); + printf( "Error - NULL library name specified for model %s\n", token ); else LoadControllerModule( lib ); } Modified: code/stage/trunk/libstage/option.cc =================================================================== --- code/stage/trunk/libstage/option.cc 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/libstage/option.cc 2009-01-03 00:03:56 UTC (rev 7224) @@ -20,6 +20,7 @@ void Option::Load( Worldfile* wf, int section ) { + printf( "loading wf key %s\n", wf_token.c_str() ); set( (bool)wf->ReadInt( section, wf_token.c_str(), value )); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-02 20:57:39 UTC (rev 7223) +++ code/stage/trunk/libstage/stage.hh 2009-01-03 00:03:56 UTC (rev 7224) @@ -3,7 +3,7 @@ /* * Stage : a multi-robot simulator. Part of the Player Project * - * Copyright (C) 2001-2008 Richard Vaughan, Brian Gerkey, Andrew + * Copyright (C) 2001-2009 Richard Vaughan, Brian Gerkey, Andrew * Howard, Toby Collett, Reed Hedges, Alex Couture-Beil, Jeremy * Asher, Pooya Karimian * @@ -1210,552 +1210,7 @@ typedef int ctrlinit_t( StgModel* mod ); //typedef void ctrlupdate_t( StgModel* mod ); - /// %StgModel class - class StgModel : public StgAncestor - { - friend class StgAncestor; - friend class StgWorld; - friend class StgWorldGui; - friend class StgCanvas; - friend class StgBlock; - friend class Region; - friend class BlockGroup; - - private: - /** the number of models instatiated - used to assign unique IDs */ - static uint32_t count; - static GHashTable* modelsbyid; - std::vector<Option*> drawOptions; - const std::vector<Option*>& getOptions() const { return drawOptions; } - protected: - //static const char* typestr; - - GMutex* access_mutex; - GPtrArray* blinkenlights; - bool blob_return; - BlockGroup blockgroup; - /** OpenGL display list identifier for the blockgroup */ - int blocks_dl; - - int boundary; - - /** callback functions can be attached to any field in this - structure. When the internal function model_change(<mod>,<field>) - is called, the callback is triggered */ - GHashTable* callbacks; - - stg_color_t color; - /** This can be set to indicate that the model has new data that - may be of interest to users. This allows polling the model - instead of adding a data callback. */ - bool data_fresh; - stg_bool_t disabled; //< if non-zero, the model is disabled - int fiducial_key; - int fiducial_return; - GList* flag_list; - stg_geom_t geom; - stg_pose_t global_pose; - bool gpose_dirty; //< set this to indicate that global pose may have changed - bool gripper_return; - int gui_grid; - int gui_mask; - int gui_nose; - int gui_outline; - bool has_default_block; - /* hooks for attaching special callback functions (not used as - variables) */ - char hook_load; - char hook_save; - char hook_shutdown; - char hook_startup; - char hook_update; - /** unique process-wide identifier for this model */ - uint32_t id; - ctrlinit_t* initfunc; - stg_usec_t interval; //< time between updates in us - stg_laser_return_t laser_return; - stg_usec_t last_update; //< time of last update in us - bool map_caches_are_invalid; - stg_meters_t map_resolution; - stg_kg_t mass; - bool obstacle_return; - bool on_update_list; - bool on_velocity_list; - StgModel* parent; //< the model that owns this one, possibly NUL - /** GData datalist can contain arbitrary named data items. Can be used - by derived model types to store properties, and for user code - to associate arbitrary items with a model. */ - stg_pose_t pose; - GData* props; - bool ranger_return; - bool rebuild_displaylist; //< iff true, regenerate block display list before redraw - char* say_string; //< if non-null, this string is displayed in the GUI - - stg_bool_t stall; - /** Thread safety flag. Iff true, Update() may be called in - parallel with other models. Defaults to false for safety */ - int subs; //< the number of subscriptions to this model - bool thread_safe; - GArray* trail; - stg_model_type_t type; - bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() - stg_velocity_t velocity; - stg_watts_t watts; //< power consumed by this model - Worldfile* wf; - int wf_entity; - StgWorld* world; // pointer to the world in which this model exists - - public: - void Lock() - { - if( access_mutex == NULL ) - access_mutex = g_mutex_new(); - - assert( access_mutex ); - g_mutex_lock( access_mutex ); - } - - void Unlock() - { - assert( access_mutex ); - g_mutex_unlock( access_mutex ); - } - - protected: - - /// Register an Option for pickup by the GUI - void registerOption( Option* opt ) - { drawOptions.push_back( opt ); } - - /** Check to see if the current pose will yield a collision with - obstacles. Returns a pointer to the first entity we are in - collision with, or NULL if no collision exists. */ - StgModel* TestCollision(); - - void CommitTestedPose(); - - void Map(); - void UnMap(); - - void MapWithChildren(); - void UnMapWithChildren(); - - int TreeToPtrArray( GPtrArray* array ); - - /** raytraces a single ray from the point and heading identified by - pose, in local coords */ - stg_raytrace_result_t Raytrace( const stg_pose_t &pose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); - - /** raytraces multiple rays around the point and heading identified - by pose, in local coords */ - void Raytrace( const stg_pose_t &pose, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); - - stg_raytrace_result_t Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); - - void Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); - - - /** Causes this model and its children to recompute their global - position instead of using a cached pose in - StgModel::GetGlobalPose()..*/ - void GPoseDirtyTree(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void UpdatePose(); - - void StartUpdating(); - void StopUpdating(); - - StgModel* ConditionalMove( stg_pose_t newpose ); - - stg_meters_t ModelHeight(); - - bool UpdateDue( void ); - void UpdateIfDue(); - - void DrawBlocksTree(); - virtual void DrawBlocks(); - void DrawBoundingBox(); - void DrawBoundingBoxTree(); - virtual void DrawStatus( Camera* cam ); - void DrawStatusTree( Camera* cam ); - - void DrawOriginTree(); - void DrawOrigin(); - - void PushLocalCoords(); - void PopCoords(); - - ///Draw the image stored in texture_id above the model - void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); - - - // static wrapper for DrawBlocks() - static void DrawBlocks( gpointer dummykey, - StgModel* mod, - void* arg ); - - virtual void DrawPicker(); - virtual void DataVisualize( Camera* cam ); - - virtual void DrawSelected(void); - - void DrawTrailFootprint(); - void DrawTrailBlocks(); - void DrawTrailArrows(); - void DrawGrid(); - - - void DrawBlinkenlights(); - - void DataVisualizeTree( Camera* cam ); - - virtual void PushColor( stg_color_t col ) - { world->PushColor( col ); } - - virtual void PushColor( double r, double g, double b, double a ) - { world->PushColor( r,g,b,a ); } - - virtual void PopColor(){ world->PopColor(); } - - void DrawFlagList(); -// stg_point_t* LocalToGlobal( double scalex, -// double scaley, -// stg_point_t pts[], -// uint32_t pt_count ); - - void DrawPose( stg_pose_t pose ); - - public: - void RecordRenderPoint( GSList** head, GSList* link, - unsigned int* c1, unsigned int* c2 ); - - void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, - stg_meters_t ymin, stg_meters_t ymax ); - - /** Look up a model pointer by a unique model ID */ - static StgModel* LookupId( uint32_t id ) - { return (StgModel*)g_hash_table_lookup( modelsbyid, (void*)id ); } - - /** Constructor */ - StgModel( StgWorld* world, - StgModel* parent, - stg_model_type_t type = MODEL_TYPE_PLAIN ); - - /** Destructor */ - virtual ~StgModel(); - - void Say( const char* str ); - - void Load( Worldfile* wf, int wf_entity ) - { - /** Set the worldfile and worldfile entity ID - must be called - before Load() */ - SetWorldfile( wf, wf_entity ); - Load(); // call virtual load - } - - /** Set the worldfile and worldfile entity ID */ - void SetWorldfile( Worldfile* wf, int wf_entity ) - { this->wf = wf; this->wf_entity = wf_entity; } - - /** configure a model by reading from the current world file */ - virtual void Load(); - - /** save the state of the model to the current world file */ - virtual void Save(); - - /** Should be called after all models are loaded, to do any last-minute setup */ - void Init(); - void InitRecursive(); - - void AddFlag( StgFlag* flag ); - void RemoveFlag( StgFlag* flag ); - - void PushFlag( StgFlag* flag ); - StgFlag* PopFlag(); - - int GetFlagCount(){ return g_list_length( flag_list ); } - - void AddBlinkenlight( stg_blinkenlight_t* b ) - { - g_ptr_array_add( this->blinkenlights, b ); - } - - void ClearBlinkenlights() - { - g_ptr_array_set_size( this->blinkenlights, 0 ); - } - - void Enable(){ disabled = false; }; - void Disable(){ disabled = true; }; - - // Load a control program for this model from an external library - void LoadControllerModule( char* lib ); - - // call this to ensure the GUI window is redrawn - void NeedRedraw(); - - void LoadBlock( Worldfile* wf, int entity ); - - // void AddBlock( stg_point_t* pts, - // size_t pt_count, - // stg_meters_t zmin, - // stg_meters_t zmax, - // stg_color_t color, - // bool inherit_color ); - - void AddBlockRect( stg_meters_t x, stg_meters_t y, - stg_meters_t dx, stg_meters_t dy, - stg_meters_t dz ); - - - /** remove all blocks from this model, freeing their memory */ - void ClearBlocks(); - - //const char* TypeStr(){ return this->typestr; } - StgModel* Parent(){ return this->parent; } - StgModel* GetModel( const char* name ); - int GuiMask(){ return this->gui_mask; }; - inline StgWorld* GetWorld(){ return this->world; } - - /// return the root model of the tree containing this model - StgModel* Root() - { return( parent ? parent->Root() : this ); } - - bool ObstacleReturn(){ return obstacle_return; } - stg_laser_return_t GetLaserReturn(){ return laser_return; } - int GetRangerReturn(){ return ranger_return; } - int FiducialReturn(){ return fiducial_return; } - int FiducialKey(){ return fiducial_key; } - - bool IsAntecedent( StgModel* testmod ); - - // returns true if model [testmod] is a descendent of model [mod] - bool IsDescendent( StgModel* testmod ); - - bool IsRelated( StgModel* mod2 ); - - /** get the pose of a model in the global CS */ - stg_pose_t GetGlobalPose(); - - /** get the velocity of a model in the global CS */ - stg_velocity_t GetGlobalVelocity(); - - /* set the velocity of a model in the global coordinate system */ - void SetGlobalVelocity( stg_velocity_t gvel ); - - /** subscribe to a model's data */ - void Subscribe(); - - /** unsubscribe from a model's data */ - void Unsubscribe(); - - /** set the pose of model in global coordinates */ - void SetGlobalPose( stg_pose_t gpose ); - - /** set a model's velocity in its parent's coordinate system */ - void SetVelocity( stg_velocity_t vel ); - - /** set a model's pose in its parent's coordinate system */ - void SetPose( stg_pose_t pose ); - - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( stg_pose_t pose ); - - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( double dx, double dy, double dz, double da ); - - /** set a model's geometry (size and center offsets) */ - void SetGeom( stg_geom_t src ); - - /** set a model's geometry (size and center offsets) */ - void SetFiducialReturn( int fid ); - - /** set a model's fiducial key: only fiducial finders with a - matching key can detect this model as a fiducial. */ - void SetFiducialKey( int key ); - - stg_color_t GetColor(){ return color; } - - // stg_laser_return_t GetLaserReturn(){ return laser_return; } - - /** Change a model's parent - experimental*/ - int SetParent( StgModel* newparent); - - /** Get a model's geometry - it's size and local pose (offset from - origin in local coords) */ - stg_geom_t GetGeom(){ return geom; } - - /** Get the pose of a model in its parent's coordinate system */ - stg_pose_t GetPose(){ return pose; } - - /** Get a model's velocity (in its local reference frame) */ - stg_velocity_t GetVelocity(){ return velocity; } - - // guess what these do? - void SetColor( stg_color_t col ); - void SetMass( stg_kg_t mass ); - void SetStall( stg_bool_t stall ); - void SetGripperReturn( int val ); - void SetLaserReturn( stg_laser_return_t val ); - void SetObstacleReturn( int val ); - void SetBlobReturn( int val ); - void SetRangerReturn( int val ); - void SetBoundary( int val ); - void SetGuiNose( int val ); - void SetGuiMask( int val ); - void SetGuiGrid( int val ); - void SetGuiOutline( int val ); - void SetWatts( stg_watts_t watts ); - void SetMapResolution( stg_meters_t res ); - - bool DataIsFresh(){ return this->data_fresh; } - - /* attach callback functions to data members. The function gets - called when the member is changed using SetX() accessor method */ - - void AddCallback( void* address, - stg_model_callback_t cb, - void* user ); - - int RemoveCallback( void* member, - stg_model_callback_t callback ); - - void CallCallbacks( void* address ); - - /* wrappers for the generic callback add & remove functions that hide - some implementation detail */ - - void AddStartupCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hook_startup, cb, user ); }; - - void RemoveStartupCallback( stg_model_callback_t cb ) - { RemoveCallback( &hook_startup, cb ); }; - - void AddShutdownCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hook_shutdown, cb, user ); }; - - void RemoveShutdownCallback( stg_model_callback_t cb ) - { RemoveCallback( &hook_shutdown, cb ); } - - void AddLoadCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hook_load, cb, user ); } - - void RemoveLoadCallback( stg_model_callback_t cb ) - { RemoveCallback( &hook_load, cb ); } - - void AddSaveCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hook_save, cb, user ); } - - void RemoveSaveCallback( stg_model_callback_t cb ) - { RemoveCallback( &hook_save, cb ); } - - void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hook_update, cb, user ); } - - void RemoveUpdateCallback( stg_model_callback_t cb ) - { RemoveCallback( &hook_update, cb ); } - - /** named-property interface - */ - void* GetProperty( char* key ); - - /** @brief Set a named property of a Stage model. - - Set a property of a Stage model. - - This function can set both predefined and user-defined - properties of a model. Predefined properties are intrinsic to - every model, such as pose and color. Every supported predefined - properties has its identifying string defined as a preprocessor - macro in stage.h. Users should use the macro instead of a - hard-coded string, so that the compiler can help you to avoid - mis-naming properties. - - User-defined properties allow the user to attach arbitrary data - pointers to a model. User-defined property data is not copied, - so the original pointer must remain valid. User-defined property - names are simple strings. Names beginning with an underscore - ('_') are reserved for internal libstage use: users should not - use names beginning with underscore (at risk of causing very - weird behaviour). - - Any callbacks registered for the named property will be called. - - Returns 0 on success, or a positive error code on failure. - - *CAUTION* The caller is responsible for making sure the pointer - points to data of the correct type for the property, so use - carefully. Check the docs or the equivalent - stg_model_set_<property>() function definition to see the type - of data required for each property. - */ - int SetProperty( char* key, void* data ); - void UnsetProperty( char* key ); - - virtual void Print( char* prefix ); - virtual const char* PrintWithPose(); - - /** Convert a pose in the world coordinate system into a model's - local coordinate system. Overwrites [pose] with the new - coordinate. */ - stg_pose_t GlobalToLocal( const stg_pose_t pose ); - - /** Convert a pose in the model's local coordinate system into the - world coordinate system. Overwrites [pose] with the new - coordinate. */ - //void LocalToGlobal( stg_pose_t* pose ); - - /** Return the global pose (i.e. pose in world coordinates) of a - pose specified in the model's local coordinate system */ - stg_pose_t LocalToGlobal( const stg_pose_t 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 ); - - /** returns the first descendent of this model that is unsubscribed - and has the type indicated by the string */ - StgModel* GetUnsubscribedModelOfType( const stg_model_type_t type ); - - /** returns the first descendent of this model that is unused - and has the type indicated by the string. This model is tagged as used. */ - StgModel* GetUnusedModelOfType( const stg_model_type_t type ); - - // Iff true, model may output some debugging visualizations and other info - //bool debug; - - /** Returns the value of the model's stall boolean, which is true - iff the model has crashed into another model */ - bool Stalled(){ return this->stall; } - }; - // BLOCKS class Camera @@ -1961,564 +1416,8 @@ }; - // BLOBFINDER MODEL -------------------------------------------------------- +#include "model.hh" - /** blobfinder data packet - */ - typedef struct - { - stg_color_t color; - uint32_t left, top, right, bottom; - stg_meters_t range; - } stg_blobfinder_blob_t; - - /// %StgModelBlobfinder class - class StgModelBlobfinder : public StgModel - { - private: - GArray* colors; - GArray* blobs; - - // predicate for ray tracing - static bool BlockMatcher( StgBlock* testblock, StgModel* finder ); - - static Option showBlobData; - - virtual void DataVisualize( Camera* cam ); - - public: - unsigned int scan_width; - unsigned int scan_height; - stg_meters_t range; - stg_radians_t fov; - stg_radians_t pan; - - static const char* typestr; - - // constructor - StgModelBlobfinder( StgWorld* world, - StgModel* parent ); - // destructor - ~StgModelBlobfinder(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void Load(); - - stg_blobfinder_blob_t* GetBlobs( unsigned int* count ) - { - if( count ) *count = blobs->len; - return (stg_blobfinder_blob_t*)blobs->data; - } - - /** Start finding blobs with this color.*/ - void AddColor( stg_color_t col ); - - /** Stop tracking blobs with this color */ - void RemoveColor( stg_color_t col ); - - /** Stop tracking all colors. Call this to clear the defaults, then - add colors individually with AddColor(); */ - void RemoveAllColors(); - }; - - // ENERGY model -------------------------------------------------------------- - - /** energy data packet */ - typedef struct - { - /** estimate of current energy stored */ - stg_joules_t stored; - - /** TRUE iff the device is receiving energy from a charger */ - stg_bool_t charging; - - /** diatance to charging device */ - stg_meters_t range; - - /** an array of pointers to connected models */ - GPtrArray* connections; - } stg_energy_data_t; - - /** energy config packet (use this to set or get energy configuration)*/ - typedef struct - { - /** maximum storage capacity */ - stg_joules_t capacity; - - /** When charging another device, supply this many Joules/sec at most*/ - stg_watts_t give_rate; - - /** When charging from another device, receive this many Joules/sec at most*/ - stg_watts_t take_rate; - - /** length of the charging probe */ - stg_meters_t probe_range; - - /** iff TRUE, this device will supply power to connected devices */ - stg_bool_t give; - - } stg_energy_config_t; - - // there is currently no energy command packet - - - // LASER MODEL -------------------------------------------------------- - - /** laser sample packet - */ - typedef struct - { - stg_meters_t range; ///< range to laser hit in meters - double reflectance; ///< intensity of the reflection 0.0 to 1.0 - } stg_laser_sample_t; - - typedef struct - { - uint32_t sample_count; - uint32_t resolution; - stg_bounds_t range_bounds; - stg_radians_t fov; - stg_usec_t interval; - } stg_laser_cfg_t; - - /// %StgModelLaser class - class StgModelLaser : public StgModel - { - private: - /** OpenGL displaylist for laser data */ - int data_dl; - bool da... [truncated message content] |
From: <rt...@us...> - 2009-01-03 23:39:33
|
Revision: 7229 http://playerstage.svn.sourceforge.net/playerstage/?rev=7229&view=rev Author: rtv Date: 2009-01-03 23:39:30 +0000 (Sat, 03 Jan 2009) Log Message: ----------- cleaning up Modified Paths: -------------- code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-03 02:05:42 UTC (rev 7228) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-03 23:39:30 UTC (rev 7229) @@ -5,7 +5,6 @@ sink source wander - trail ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-03 02:05:42 UTC (rev 7228) +++ code/stage/trunk/libstage/stage.hh 2009-01-03 23:39:30 UTC (rev 7229) @@ -158,31 +158,25 @@ */ const double billion = 1e9; - /** convert an angle in radians to degrees - */ + /** convert an angle in radians to degrees. */ inline double rtod( double r ){ return( r*180.0/M_PI ); } - /** convert an angle in degrees to radians - */ + /** convert an angle in degrees to radians. */ inline double dtor( double d ){ return( d*M_PI/180.0 ); } - /** Normalize an angle to within +/_ M_PI - */ + /** Normalize an angle to within +/_ M_PI. */ double normalize( double a ); - /** take binary sign of a, either -1, or 1 if >= 0 - */ + /** take binary sign of a, either -1, or 1 if >= 0 */ inline int sgn( int a){ return( a<0 ? -1 : 1); } - /** take binary sign of a, either -1, or 1 if >= 0 - */ + /** take binary sign of a, either -1, or 1 if >= 0. */ inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); } // forward declare class StgModel; - /** Describe the image format used for saving screenshots - */ + /** Describe the image format used for saving screenshots. */ typedef enum { STG_IMAGE_FORMAT_PNG, STG_IMAGE_FORMAT_JPG @@ -191,7 +185,7 @@ /** any integer value other than this is a valid fiducial ID */ enum { FiducialNone = 0 }; - /** Value that Uniquely identifies a model */ + /** Value that uniquely identifies a model */ typedef uint32_t stg_id_t; /** Metres: floating point unit of distance */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-05 02:18:42
|
Revision: 7240 http://playerstage.svn.sourceforge.net/playerstage/?rev=7240&view=rev Author: rtv Date: 2009-01-05 02:18:00 +0000 (Mon, 05 Jan 2009) Log Message: ----------- API changes! removed the Stg prefix from all class names, and renamed some key classes, eg. Stg::stg_pose_t is now Stg::Pose Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/examples/ctrl/lasernoise.cc code/stage/trunk/examples/ctrl/sink.cc code/stage/trunk/examples/ctrl/source.cc code/stage/trunk/examples/ctrl/wander.cc code/stage/trunk/examples/libstage/stest.cc code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgrid.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/resource.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/test.cc code/stage/trunk/libstage/typetable.cc code/stage/trunk/libstage/waypoint.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/libstageplugin/p_fiducial.cc code/stage/trunk/libstageplugin/p_graphics3d.cc code/stage/trunk/libstageplugin/p_gripper.cc code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/libstageplugin/p_localize.cc code/stage/trunk/libstageplugin/p_map.cc code/stage/trunk/libstageplugin/p_position.cc code/stage/trunk/libstageplugin/p_ptz.cc code/stage/trunk/libstageplugin/p_simulation.cc code/stage/trunk/worlds/benchmark/expand.cc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -29,37 +29,34 @@ typedef struct { - StgModelPosition* pos; - StgModelLaser* laser; - StgModelRanger* ranger; - StgModelBlobfinder* blobfinder; - - StgModel *source, *sink; - + ModelPosition* pos; + ModelLaser* laser; + ModelRanger* ranger; + ModelBlobfinder* blobfinder; + Model *source, *sink; int avoidcount, randcount; - int work_get, work_put; } robot_t; -int LaserUpdate( StgModel* mod, robot_t* robot ); -int PositionUpdate( StgModel* mod, robot_t* robot ); +int LaserUpdate( Model* mod, robot_t* robot ); +int PositionUpdate( Model* mod, robot_t* robot ); // Stage calls this when the model starts up -extern "C" int Init( StgModel* mod ) +extern "C" int Init( Model* mod ) { robot_t* robot = new robot_t; robot->work_get = 0; robot->work_put = 0; - robot->pos = (StgModelPosition*)mod; + robot->pos = (ModelPosition*)mod; - robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" ); + robot->laser = (ModelLaser*)mod->GetModel( "laser:0" ); assert( robot->laser ); robot->laser->Subscribe(); - robot->ranger = (StgModelRanger*)mod->GetModel( "ranger:0" ); + robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" ); assert( robot->ranger ); //robot->ranger->Subscribe(); @@ -95,7 +92,7 @@ } // inspect the laser data and decide what to do -int LaserUpdate( StgModel* mod, robot_t* robot ) +int LaserUpdate( Model* mod, robot_t* robot ) { // get the data uint32_t sample_count=0; @@ -178,7 +175,7 @@ robot->avoidcount = 0; robot->pos->SetXSpeed( cruisespeed ); - stg_pose_t pose = robot->pos->GetPose(); + Pose pose = robot->pos->GetPose(); int x = (pose.x + 8) / 4; int y = (pose.y + 8) / 4; @@ -208,9 +205,9 @@ return 0; } -int PositionUpdate( StgModel* mod, robot_t* robot ) +int PositionUpdate( Model* mod, robot_t* robot ) { - stg_pose_t pose = robot->pos->GetPose(); + Pose pose = robot->pos->GetPose(); //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", // pose.x, pose.y, pose.z, pose.a ); Modified: code/stage/trunk/examples/ctrl/lasernoise.cc =================================================================== --- code/stage/trunk/examples/ctrl/lasernoise.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/ctrl/lasernoise.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -21,7 +21,7 @@ } // process the laser data -int LaserUpdate( StgModelLaser* mod, void* dummy ) +int LaserUpdate( ModelLaser* mod, void* dummy ) { // get the data uint32_t sample_count=0; @@ -36,7 +36,7 @@ // Stage calls this when the model starts up. we just add a callback to // the model that gets called just after the sensor update is done. -extern "C" int Init( StgModel* mod ) +extern "C" int Init( Model* mod ) { mod->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, NULL ); return 0; // ok Modified: code/stage/trunk/examples/ctrl/sink.cc =================================================================== --- code/stage/trunk/examples/ctrl/sink.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/ctrl/sink.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -3,14 +3,14 @@ const int INTERVAL = 50; -int Update( StgModel* mod, void* dummy ); +int Update( Model* mod, void* dummy ); // Stage calls this when the model starts up -extern "C" int Init( StgModel* mod ) +extern "C" int Init( Model* mod ) { for( int i=0; i<5; i++ ) - mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), 0.5 ) ); + mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), 0.5 ) ); mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); @@ -18,7 +18,7 @@ } // inspect the laser data and decide what to do -int Update( StgModel* mod, void* dummy ) +int Update( Model* mod, void* dummy ) { // protect access to this model from other controllers mod->Lock(); Modified: code/stage/trunk/examples/ctrl/source.cc =================================================================== --- code/stage/trunk/examples/ctrl/source.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/ctrl/source.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -3,15 +3,15 @@ const int INTERVAL = 50; -int Update( StgModel* mod, void* dummy ); +int Update( Model* mod, void* dummy ); const double flagsz = 0.4; // Stage calls this when the model starts up -extern "C" int Init( StgModel* mod ) +extern "C" int Init( Model* mod ) { for( int i=0; i<5; i++ ) - mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0 ), flagsz ) ); + mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0 ), flagsz ) ); mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL ); @@ -19,13 +19,13 @@ } // inspect the laser data and decide what to do -int Update( StgModel* mod, void* dummy ) +int Update( Model* mod, void* dummy ) { // protect access to this model from other controllers mod->Lock(); if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 ) - mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), flagsz ) ); + mod->PushFlag( new Flag( stg_color_pack( 1,1,0,0), flagsz ) ); mod->Unlock(); Modified: code/stage/trunk/examples/ctrl/wander.cc =================================================================== --- code/stage/trunk/examples/ctrl/wander.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/ctrl/wander.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -11,24 +11,24 @@ typedef struct { - StgModelPosition* pos; - StgModelLaser* laser; + ModelPosition* pos; + ModelLaser* laser; int avoidcount, randcount; } robot_t; -int LaserUpdate( StgModel* mod, robot_t* robot ); -int PositionUpdate( StgModel* mod, robot_t* robot ); +int LaserUpdate( Model* mod, robot_t* robot ); +int PositionUpdate( Model* mod, robot_t* robot ); // Stage calls this when the model starts up -extern "C" int Init( StgModel* mod ) +extern "C" int Init( Model* mod ) { robot_t* robot = new robot_t; robot->avoidcount = 0; robot->randcount = 0; - robot->pos = (StgModelPosition*)mod; - robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" ); + robot->pos = (ModelPosition*)mod; + robot->laser = (ModelLaser*)mod->GetModel( "laser:0" ); robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot ); robot->laser->Subscribe(); // starts the laser @@ -37,7 +37,7 @@ // inspect the laser data and decide what to do -int LaserUpdate( StgModel* mod, robot_t* robot ) +int LaserUpdate( Model* mod, robot_t* robot ) { // get the data uint32_t sample_count=0; @@ -123,9 +123,9 @@ return 0; } -int PositionUpdate( StgModel* mod, robot_t* robot ) +int PositionUpdate( Model* mod, robot_t* robot ) { - stg_pose_t pose = robot->pos->GetPose(); + Pose pose = robot->pos->GetPose(); printf( "Pose: [%.2f %.2f %.2f %.2f]\n", pose.x, pose.y, pose.z, pose.a ); Modified: code/stage/trunk/examples/libstage/stest.cc =================================================================== --- code/stage/trunk/examples/libstage/stest.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/examples/libstage/stest.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -21,11 +21,11 @@ typedef struct { - StgModelLaser* laser; - StgModelPosition* position; - StgModelRanger* ranger; - StgModelFiducial* fiducial; - StgModelBlobfinder* blobfinder; + ModelLaser* laser; + ModelPosition* position; + ModelRanger* ranger; + ModelFiducial* fiducial; + ModelBlobfinder* blobfinder; } robot_t; #define VSPEED 0.4 // meters per second @@ -59,26 +59,26 @@ { char* base = "r"; sprintf( namebuf, "%s%d", base, i ); - robots[i].position = (StgModelPosition*)world.GetModel( namebuf ); + robots[i].position = (ModelPosition*)world.GetModel( namebuf ); assert(robots[i].position); robots[i].position->Subscribe(); -// robots[i].laser = (StgModelLaser*) +// robots[i].laser = (ModelLaser*) // robots[i].position->GetUnsubscribedModelOfType( "laser" ); // assert(robots[i].laser); // robots[i].laser->Subscribe(); -// robots[i].fiducial = (StgModelFiducial*) +// robots[i].fiducial = (ModelFiducial*) // robots[i].position->GetUnsubscribedModelOfType( "fiducial" ); // assert(robots[i].fiducial); // robots[i].fiducial->Subscribe(); - robots[i].ranger = (StgModelRanger*) + robots[i].ranger = (ModelRanger*) robots[i].position->GetUnsubscribedModelOfType( "ranger" ); assert(robots[i].ranger); robots[i].ranger->Subscribe(); -// robots[i].blobfinder = (StgModelBlobfinder*) +// robots[i].blobfinder = (ModelBlobfinder*) // robots[i].position->GetUnsubscribedModelOfType( "blobfinder" ); // assert(robots[i].blobfinder); // robots[i].blobfinder->Subscribe(); @@ -96,7 +96,7 @@ //continue; - StgModelRanger* rgr = robots[i].ranger; + ModelRanger* rgr = robots[i].ranger; if( rgr->samples == NULL ) continue; Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/ancestor.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -1,6 +1,6 @@ #include "stage_internal.hh" -StgAncestor::StgAncestor() +Ancestor::Ancestor() { token = NULL; children = NULL; @@ -11,19 +11,19 @@ debug = false; } -StgAncestor::~StgAncestor() +Ancestor::~Ancestor() { if( children ) { for( GList* it = children; it; it=it->next ) - delete (StgModel*)it->data; + delete (Model*)it->data; g_list_free( children ); } } -void StgAncestor::AddChild( StgModel* mod ) +void Ancestor::AddChild( Model* mod ) { // poke a name into the child @@ -50,23 +50,23 @@ delete[] buf; } -void StgAncestor::RemoveChild( StgModel* mod ) +void Ancestor::RemoveChild( Model* mod ) { child_type_counts[mod->type]--; children = g_list_remove( children, mod ); } -stg_pose_t StgAncestor::GetGlobalPose() +Pose Ancestor::GetGlobalPose() { - stg_pose_t pose; + Pose pose; bzero( &pose, sizeof(pose)); return pose; } -void StgAncestor::ForEachDescendant( stg_model_callback_t func, void* arg ) +void Ancestor::ForEachDescendant( stg_model_callback_t func, void* arg ) { for( GList* it=children; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; + Model* mod = (Model*)it->data; func( mod, arg ); mod->ForEachDescendant( func, arg ); } Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/block.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -1,12 +1,12 @@ #include "stage_internal.hh" -//GPtrArray* StgBlock::global_verts = g_ptr_array_sized_new( 1024 ); +//GPtrArray* Block::global_verts = g_ptr_array_sized_new( 1024 ); /** Create a new block. A model's body is a list of these blocks. The point data is copied, so pts can safely be freed after calling this.*/ -StgBlock::StgBlock( StgModel* mod, +Block::Block( Model* mod, stg_point_t* pts, size_t pt_count, stg_meters_t zmin, @@ -35,7 +35,7 @@ } /** A from-file constructor */ -StgBlock::StgBlock( StgModel* mod, +Block::Block( Model* mod, Worldfile* wf, int entity) : mod( mod ), @@ -58,7 +58,7 @@ } -StgBlock::~StgBlock() +Block::~Block() { if( mapped ) UnMap(); @@ -71,14 +71,14 @@ //g_ptr_array_remove( global_verts, this ); } -stg_color_t StgBlock::GetColor() +stg_color_t Block::GetColor() { return( inherit_color ? mod->color : color ); } -StgModel* StgBlock::TestCollision() +Model* Block::TestCollision() { //printf( "model %s block %p test collision...\n", mod->Token(), this ); @@ -93,14 +93,14 @@ // for every rendered into that cell for( GSList* it = cell->list; it; it=it->next ) { - StgBlock* testblock = (StgBlock*)it->data; - StgModel* testmod = testblock->mod; + Block* testblock = (Block*)it->data; + Model* testmod = testblock->mod; //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); // if the tested model is an obstacle and it's not attached to this model if( (testmod != this->mod) && - testmod->obstacle_return && + testmod->vis.obstacle_return && !mod->IsRelated( testmod )) { //puts( "HIT"); @@ -114,13 +114,13 @@ } -void StgBlock::RemoveFromCellArray( GPtrArray* ptrarray ) +void Block::RemoveFromCellArray( GPtrArray* ptrarray ) { for( unsigned int i=0; i<ptrarray->len; i++ ) ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); } -void StgBlock::AddToCellArray( GPtrArray* ptrarray ) +void Block::AddToCellArray( GPtrArray* ptrarray ) { for( unsigned int i=0; i<ptrarray->len; i++ ) ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); @@ -134,12 +134,12 @@ } // used as a callback to gather an array of cells in a polygon -void AddBlockToCell( Cell* c, StgBlock* block ) +void AddBlockToCell( Cell* c, Block* block ) { c->AddBlock( block ); } -void StgBlock::Map() +void Block::Map() { // TODO - if called often, we may not need to generate each time GenerateCandidateCells(); @@ -149,7 +149,7 @@ mapped = true; } -void StgBlock::UnMap() +void Block::UnMap() { RemoveFromCellArray( rendered_cells ); @@ -157,7 +157,7 @@ mapped = false; } -void StgBlock::SwitchToTestedCells() +void Block::SwitchToTestedCells() { RemoveFromCellArray( rendered_cells ); AddToCellArray( candidate_cells ); @@ -170,15 +170,15 @@ mapped = true; } -void StgBlock::GenerateCandidateCells() +void Block::GenerateCandidateCells() { - stg_pose_t gpose = mod->GetGlobalPose(); + Pose gpose = mod->GetGlobalPose(); // add local offset gpose = pose_sum( gpose, mod->geom.pose ); - stg_size_t bgsize = mod->blockgroup.GetSize(); + Size bgsize = mod->blockgroup.GetSize(); stg_point3_t bgoffset = mod->blockgroup.GetOffset(); stg_point3_t scale; @@ -190,12 +190,12 @@ g_ptr_array_set_size( candidate_cells, 0 ); // compute the global location of the first point - stg_pose_t local( (pts[0].x - bgoffset.x) * scale.x , + Pose local( (pts[0].x - bgoffset.x) * scale.x , (pts[0].y - bgoffset.y) * scale.y, -bgoffset.z, 0 ); - stg_pose_t first_gpose, last_gpose; + Pose first_gpose, last_gpose; first_gpose = last_gpose = pose_sum( gpose, local ); // store the block's absolute z bounds at this rendering @@ -205,12 +205,12 @@ // now loop from the the second to the last for( unsigned int p=1; p<pt_count; p++ ) { - stg_pose_t local( (pts[p].x - bgoffset.x) * scale.x , + Pose local( (pts[p].x - bgoffset.x) * scale.x , (pts[p].y - bgoffset.y) * scale.y, -bgoffset.z, 0 ); - stg_pose_t gpose2 = pose_sum( gpose, local ); + Pose gpose2 = pose_sum( gpose, local ); // and render the shape of the block into the global cells mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, @@ -230,7 +230,7 @@ } -void StgBlock::DrawTop() +void Block::DrawTop() { // draw the top of the block - a polygon at the highest vertical // extent @@ -240,7 +240,7 @@ glEnd(); } -void StgBlock::DrawSides() +void Block::DrawSides() { // construct a strip that wraps around the polygon glBegin(GL_QUAD_STRIP); @@ -255,7 +255,7 @@ glEnd(); } -void StgBlock::DrawFootPrint() +void Block::DrawFootPrint() { glBegin(GL_POLYGON); for( unsigned int p=0; p<pt_count; p++ ) @@ -263,7 +263,7 @@ glEnd(); } -void StgBlock::Draw( StgModel* mod ) +void Block::Draw( Model* mod ) { // draw filled color polygons stg_color_t col = inherit_color ? mod->color : color; @@ -291,7 +291,7 @@ mod->PopColor(); } -void StgBlock::DrawSolid() +void Block::DrawSolid() { DrawSides(); DrawTop(); @@ -301,9 +301,9 @@ //#define DEBUG 1 -void StgBlock::Load( Worldfile* wf, int entity ) +void Block::Load( Worldfile* wf, int entity ) { - //printf( "StgBlock::Load entity %d\n", entity ); + //printf( "Block::Load entity %d\n", entity ); if( pts ) stg_points_destroy( pts ); Modified: code/stage/trunk/libstage/blockgrid.cc =================================================================== --- code/stage/trunk/libstage/blockgrid.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/blockgrid.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -7,7 +7,7 @@ static const uint32_t NSQR = NSIZE*NSIZE; static const uint32_t MASK = NSIZE-1; -StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height ) +BlockGrid::BlockGrid( uint32_t width, uint32_t height ) { this->width = width; this->height = height; @@ -16,7 +16,7 @@ assert(this->cells); } -StgBlockGrid::~StgBlockGrid() +BlockGrid::~BlockGrid() { for( uint32_t x=0; x<width; x++ ) for( uint32_t y=0; y<height; y++ ) @@ -29,7 +29,7 @@ //delete[] map; } -void StgBlockGrid::AddBlock( uint32_t x, uint32_t y, StgBlock* block ) +void BlockGrid::AddBlock( uint32_t x, uint32_t y, Block* block ) { //printf( "add block %u %u\n", x, y ); @@ -40,7 +40,7 @@ } } -void StgBlockGrid::RemoveBlock( uint32_t x, uint32_t y, StgBlock* block ) +void BlockGrid::RemoveBlock( uint32_t x, uint32_t y, Block* block ) { //printf( "remove block %u %u\n", x, y ); @@ -50,7 +50,7 @@ } } -GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y ) +GSList* BlockGrid::GetList( uint32_t x, uint32_t y ) { if( x < width && y < height ) return cells[ x+y*width ]; @@ -58,14 +58,14 @@ return NULL; } -void StgBlockGrid::GlobalRemoveBlock( StgBlock* block ) +void BlockGrid::GlobalRemoveBlock( Block* block ) { for( uint32_t x=0; x<width; x++ ) for( uint32_t y=0; y<height; y++ ) RemoveBlock(x,y,block ); } -void StgBlockGrid::Draw( bool drawall ) +void BlockGrid::Draw( bool drawall ) { for( uint32_t x=0; x<width; x++ ) for( uint32_t y=0; y<height; y++ ) Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/blockgroup.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -21,7 +21,7 @@ Clear(); } -void BlockGroup::AppendBlock( StgBlock* block ) +void BlockGroup::AppendBlock( Block* block ) { blocks = g_list_append( blocks, block ); ++count; @@ -33,7 +33,7 @@ { while( blocks ) { - delete (StgBlock*)blocks->data; + delete (Block*)blocks->data; blocks = blocks->next; } @@ -45,17 +45,17 @@ void BlockGroup::SwitchToTestedCells() { // confirm the tentative pose for all blocks - LISTMETHOD( blocks, StgBlock*, SwitchToTestedCells ); + LISTMETHOD( blocks, Block*, SwitchToTestedCells ); } -StgModel* BlockGroup::TestCollision() +Model* BlockGroup::TestCollision() { //printf( "blockgroup %p test collision...\n", this ); - StgModel* hitmod = NULL; + Model* hitmod = NULL; for( GList* it=blocks; it; it = it->next ) - if( (hitmod = ((StgBlock*)it->data)->TestCollision())) + if( (hitmod = ((Block*)it->data)->TestCollision())) break; // bail on the earliest collision //printf( "blockgroup %p test collision done.\n", this ); @@ -76,12 +76,12 @@ size.z = 0.0; // grow to largest z we see if( blocks ) - ((StgBlock*)blocks->data)->mod->map_caches_are_invalid = true; + ((Block*)blocks->data)->mod->map_caches_are_invalid = true; for( GList* it=blocks; it; it=it->next ) // examine all the blocks { // examine all the points in the polygon - StgBlock* block = (StgBlock*)it->data; + Block* block = (Block*)it->data; for( unsigned int p=0; p < block->pt_count; p++ ) { @@ -105,21 +105,21 @@ // normalize blocks // for( GList* it = blocks; itl it=it->next ) - //((StgBlock*)it->data)->Normalize( size.x, size.y, size.z, offset.x + //((Block*)it->data)->Normalize( size.x, size.y, size.z, offset.x } void BlockGroup::Map() { - LISTMETHOD( blocks, StgBlock*, Map ); + LISTMETHOD( blocks, Block*, Map ); } void BlockGroup::UnMap() { - LISTMETHOD( blocks, StgBlock*, UnMap ); + LISTMETHOD( blocks, Block*, UnMap ); } -void BlockGroup::DrawSolid( const stg_geom_t & geom ) +void BlockGroup::DrawSolid( const Geom & geom ) { glPushMatrix(); @@ -131,12 +131,12 @@ glTranslatef( -offset.x, -offset.y, -offset.z ); - LISTMETHOD( blocks, StgBlock*, DrawSolid ); + LISTMETHOD( blocks, Block*, DrawSolid ); glPopMatrix(); } -void BlockGroup::DrawFootPrint( const stg_geom_t & geom ) +void BlockGroup::DrawFootPrint( const Geom & geom ) { glPushMatrix(); @@ -146,12 +146,12 @@ glTranslatef( -offset.x, -offset.y, -offset.z ); - LISTMETHOD( blocks, StgBlock*, DrawFootPrint); + LISTMETHOD( blocks, Block*, DrawFootPrint); glPopMatrix(); } -void BlockGroup::BuildDisplayList( StgModel* mod ) +void BlockGroup::BuildDisplayList( Model* mod ) { //printf( "display list for model %s\n", mod->token ); @@ -164,7 +164,7 @@ glNewList( displaylist, GL_COMPILE ); - stg_geom_t geom = mod->GetGeom(); + Geom geom = mod->GetGeom(); gl_pose_shift( geom.pose ); @@ -182,7 +182,7 @@ for( GList* it=blocks; it; it=it->next ) { - StgBlock* blk = (StgBlock*)it->data; + Block* blk = (Block*)it->data; if( (!blk->inherit_color) && (blk->color != mod->color) ) { @@ -207,7 +207,7 @@ for( GList* it=blocks; it; it=it->next ) { - StgBlock* blk = (StgBlock*)it->data; + Block* blk = (Block*)it->data; if( (!blk->inherit_color) && (blk->color != mod->color) ) { @@ -228,7 +228,7 @@ glEndList(); } -void BlockGroup::CallDisplayList( StgModel* mod ) +void BlockGroup::CallDisplayList( Model* mod ) { if( displaylist == 0 ) BuildDisplayList( mod ); @@ -236,12 +236,12 @@ glCallList( displaylist ); } -void BlockGroup::LoadBlock( StgModel* mod, Worldfile* wf, int entity ) +void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity ) { - AppendBlock( new StgBlock( mod, wf, entity )); + AppendBlock( new Block( mod, wf, entity )); } -void BlockGroup::LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile* wf ) +void BlockGroup::LoadBitmap( Model* mod, const char* bitmapfile, Worldfile* wf ) { PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile ); @@ -312,7 +312,7 @@ // TODO fix this stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); - AppendBlock( new StgBlock( mod, + AppendBlock( new Block( mod, pts,4, 0,1, col, Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/canvas.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -31,7 +31,7 @@ static bool init_done = false; -void StgCanvas::TimerCallback( StgCanvas* c ) +void Canvas::TimerCallback( Canvas* c ) { if( c->world->dirty ) { @@ -41,11 +41,11 @@ } Fl::repeat_timeout(((double)c->interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, + (Fl_Timeout_Handler)Canvas::TimerCallback, c); } -StgCanvas::StgCanvas( StgWorldGui* world, +Canvas::Canvas( WorldGui* world, int x, int y, int width, int height) : Fl_Gl_Window( x, y, width, height ), @@ -95,7 +95,7 @@ assert( can_do( FL_ACCUM ) ); } -void StgCanvas::InitGl() +void Canvas::InitGl() { valid(1); FixViewport(w(), h()); @@ -163,12 +163,12 @@ } -StgCanvas::~StgCanvas() +Canvas::~Canvas() { // nothing to do } -StgModel* StgCanvas::getModel( int x, int y ) +Model* Canvas::getModel( int x, int y ) { // render all models in a unique color make_current(); // make sure the GL context is current @@ -185,11 +185,11 @@ // render all top-level, draggable models in a color that is their // id - for( GList* it= world->StgWorld::children; it; it=it->next ) + for( GList* it= world->World::children; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; + Model* mod = (Model*)it->data; - if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) + if( mod->gui.mask & (STG_MOVE_TRANS | STG_MOVE_ROT )) { uint8_t rByte, gByte, bByte, aByte; uint32_t modelId = mod->id; @@ -230,7 +230,7 @@ // printf("Clicked rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", rByte, gByte, bByte, aByte); // printf("-->model Id = 0x%X\n", modelId); - StgModel* mod = StgModel::LookupId( modelId ); + Model* mod = Model::LookupId( modelId ); //printf("%p %s %d %x\n", mod, mod ? mod->Token() : "(none)", id, id ); @@ -245,14 +245,14 @@ return mod; } -bool StgCanvas::selected( StgModel* mod ) { +bool Canvas::selected( Model* mod ) { if( g_list_find( selected_models, mod ) ) return true; else return false; } -void StgCanvas::select( StgModel* mod ) { +void Canvas::select( Model* mod ) { if( mod ) { last_selection = mod; @@ -263,7 +263,7 @@ } } -void StgCanvas::unSelect( StgModel* mod ) { +void Canvas::unSelect( Model* mod ) { if( mod ) { if ( GList* link = g_list_find( selected_models, mod ) ) @@ -277,16 +277,16 @@ } } -void StgCanvas::unSelectAll() { +void Canvas::unSelectAll() { // for( GList* it=selected_models; it; it=it->next ) - // ((StgModel*)it->data)->Enable(); + // ((Model*)it->data)->Enable(); g_list_free( selected_models ); selected_models = NULL; } // convert from 2d window pixel to 3d world coordinates -void StgCanvas::CanvasToWorld( int px, int py, +void Canvas::CanvasToWorld( int px, int py, double *wx, double *wy, double* wz ) { if( px <= 0 ) @@ -324,7 +324,7 @@ } -int StgCanvas::handle(int event) +int Canvas::handle(int event) { switch(event) { @@ -379,7 +379,7 @@ case FL_PUSH: // button pressed { - StgModel* mod = getModel( startx, starty ); + Model* mod = getModel( startx, starty ); startx = Fl::event_x(); starty = Fl::event_y(); selectedModel = false; @@ -443,7 +443,7 @@ // move all selected models to the mouse pointer for( GList* it = selected_models; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; + Model* mod = (Model*)it->data; mod->AddToPose( x-sx, y-sy, 0, 0 ); } } @@ -463,7 +463,7 @@ // rotate all selected models for( GList* it = selected_models; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; + Model* mod = (Model*)it->data; mod->AddToPose( 0,0,0, 0.05*(dx+dy) ); } } @@ -497,12 +497,12 @@ { // // start the timer that causes regular redraws Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, + (Fl_Timeout_Handler)Canvas::TimerCallback, this); } else { // remove the timeout - Fl::remove_timeout( (Fl_Timeout_Handler)StgCanvas::TimerCallback ); + Fl::remove_timeout( (Fl_Timeout_Handler)Canvas::TimerCallback ); } redraw(); // in case something happened that will never be @@ -579,41 +579,44 @@ } // end switch( event ) } -void StgCanvas::FixViewport(int W,int H) +void Canvas::FixViewport(int W,int H) { glLoadIdentity(); glViewport(0,0,W,H); } -void StgCanvas::AddModel( StgModel* mod ) +void Canvas::AddModel( Model* mod ) { models_sorted = g_list_append( models_sorted, mod ); } -void StgCanvas::DrawGlobalGrid() +void Canvas::DrawGlobalGrid() { + stg_bounds3d_t bounds = world->GetExtent(); -// printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n", -// bounds.x.min, bounds.x.max, -// bounds.y.min, bounds.y.max, -// bounds.z.min, bounds.z.max ); - + /* printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n", + bounds.x.min, bounds.x.max, + bounds.y.min, bounds.y.max, + bounds.z.min, bounds.z.max ); + + */ + char str[64]; - PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray - for( double i = floor(bounds.x.min); i < bounds.x.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( i, 0, 0.00, str ); - } - - for( double i = floor(bounds.y.min); i < bounds.y.max; i++) - { - snprintf( str, 16, "%d", (int)i ); - gl_draw_string( 0, i, 0.00, str ); - } - PopColor(); - + PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray + for( double i = floor(bounds.x.min); i < bounds.x.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( i, 0, 0.00, str ); + } + + for( double i = floor(bounds.y.min); i < bounds.y.max; i++) + { + snprintf( str, 16, "%d", (int)i ); + gl_draw_string( 0, i, 0.00, str ); + } + PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glEnable(GL_POLYGON_OFFSET_FILL); @@ -642,7 +645,7 @@ } //draw the floor without any grid ( for robot's perspective camera model ) -void StgCanvas::DrawFloor() +void Canvas::DrawFloor() { stg_bounds3d_t bounds = world->GetExtent(); @@ -661,12 +664,12 @@ glEnd(); } -void StgCanvas::DrawBlocks() +void Canvas::DrawBlocks() { - LISTMETHOD( models_sorted, StgModel*, DrawBlocksTree ); + LISTMETHOD( models_sorted, Model*, DrawBlocksTree ); } -void StgCanvas::DrawBoundingBoxes() +void Canvas::DrawBoundingBoxes() { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glLineWidth( 2.0 ); @@ -680,15 +683,15 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); } -inline void StgCanvas::resetCamera() +inline void Canvas::resetCamera() { float max_x = 0, max_y = 0, min_x = 0, min_y = 0; //TODO take orrientation ( `a' ) and geom.pose offset into consideration - for( GList* it=world->StgWorld::children; it; it=it->next ) { - StgModel* ptr = (StgModel*) it->data; - stg_pose_t pose = ptr->GetPose(); - stg_geom_t geom = ptr->GetGeom(); + for( GList* it=world->World::children; it; it=it->next ) { + Model* ptr = (Model*) it->data; + Pose pose = ptr->GetPose(); + Geom geom = ptr->GetGeom(); float tmp_min_x = pose.x - geom.size.x / 2.0; float tmp_max_x = pose.x + geom.size.x / 2.0; @@ -713,10 +716,10 @@ } // used to sort a list of models by inverse distance from the x,y pose in [coords] -gint compare_distance( StgModel* a, StgModel* b, double coords[2] ) +gint compare_distance( Model* a, Model* b, double coords[2] ) { - stg_pose_t a_pose = a->GetGlobalPose(); - stg_pose_t b_pose = b->GetGlobalPose(); + Pose a_pose = a->GetGlobalPose(); + Pose b_pose = b->GetGlobalPose(); double a_dist = hypot( coords[1] - a_pose.y, coords[0] - a_pose.x ); @@ -733,7 +736,7 @@ return 0; // must be the same } -void StgCanvas::renderFrame() +void Canvas::renderFrame() { //before drawing, order all models based on distance from camera float x = current_camera->x(); @@ -767,10 +770,10 @@ // out for Z. look into it. if( showOccupancy ) - ((StgWorldGui*)world)->DrawTree( false ); + ((WorldGui*)world)->DrawTree( false ); if( showTree ) - ((StgWorldGui*)world)->DrawTree( true ); + ((WorldGui*)world)->DrawTree( true ); glPopMatrix(); } @@ -778,7 +781,7 @@ if( showFootprints ) { glDisable( GL_DEPTH_TEST ); - LISTMETHOD( models_sorted, StgModel*, DrawTrailFootprint ); + LISTMETHOD( models_sorted, Model*, DrawTrailFootprint ); glEnable( GL_DEPTH_TEST ); } @@ -794,7 +797,7 @@ DrawBoundingBoxes(); // TODO - finish this properly - //LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); + //LISTMETHOD( models_sorted, Model*, DrawWaypoints ); // MOTION BLUR if( 0 )//showBlur ) @@ -877,7 +880,7 @@ // if( showTrailRise ) // { -// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { +// for( std::multimap< float, Model* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { // i->second->DrawTrailBlocks(); // } // } @@ -885,7 +888,7 @@ // if( showTrailArrows ) // { // glEnable( GL_DEPTH_TEST ); -// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { +// for( std::multimap< float, Model* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { // i->second->DrawTrailArrows(); // } @@ -893,22 +896,22 @@ for( GList* it=selected_models; it; it=it->next ) - ((StgModel*)it->data)->DrawSelected(); + ((Model*)it->data)->DrawSelected(); // useful debug - puts a point at the origin of each model - //for( GList* it = world->StgWorld::children; it; it=it->next ) - // ((StgModel*)it->data)->DrawOriginTree(); + //for( GList* it = world->World::children; it; it=it->next ) + // ((Model*)it->data)->DrawOriginTree(); // draw the model-specific visualizations if( showData ) { if ( ! visualizeAll ) { - for( GList* it = world->StgWorld::children; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree( current_camera ); + for( GList* it = world->World::children; it; it=it->next ) + ((Model*)it->data)->DataVisualizeTree( current_camera ); } else if ( selected_models ) { for( GList* it = selected_models; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree( current_camera ); + ((Model*)it->data)->DataVisualizeTree( current_camera ); } else if ( last_selection ) { last_selection->DataVisualizeTree( current_camera ); @@ -916,13 +919,13 @@ } if( showGrid ) - LISTMETHOD( models_sorted, StgModel*, DrawGrid ); + LISTMETHOD( models_sorted, Model*, DrawGrid ); if( showFlags ) - LISTMETHOD( models_sorted, StgModel*, DrawFlagList ); + LISTMETHOD( models_sorted, Model*, DrawFlagList ); if( showBlinken ) - LISTMETHOD( models_sorted, StgModel*, DrawBlinkenlights ); + LISTMETHOD( models_sorted, Model*, DrawBlinkenlights ); if( showStatus ) { @@ -933,7 +936,7 @@ if( camera.pitch() == 0 && !pCamOn ) glTranslatef( 0, 0, 0.1 ); - LISTMETHODARG( models_sorted, StgModel*, DrawStatusTree, &camera ); + LISTMETHODARG( models_sorted, Model*, DrawStatusTree, &camera ); glEnable( GL_DEPTH_TEST ); glPopMatrix(); @@ -1009,7 +1012,7 @@ } -void StgCanvas::Screenshot() +void Canvas::Screenshot() { int width = w(); @@ -1071,9 +1074,9 @@ printf( "Saved %s\n", filename ); } -void StgCanvas::perspectiveCb( Fl_Widget* w, void* p ) +void Canvas::perspectiveCb( Fl_Widget* w, void* p ) { - StgCanvas* canvas = static_cast<StgCanvas*>( w ); + Canvas* canvas = static_cast<Canvas*>( w ); Option* opt = static_cast<Option*>( p ); // pCamOn if ( opt ) { // Perspective mode is on, change camera @@ -1086,7 +1089,7 @@ canvas->invalidate(); } -void StgCanvas::createMenuItems( Fl_Menu_Bar* menu, std::string path ) +void Canvas::createMenuItems( Fl_Menu_Bar* menu, std::string path ) { showData.createMenuItem( menu, path ); // visualizeAll.createMenuItem( menu, path ); @@ -1111,7 +1114,7 @@ } -void StgCanvas::Load( Worldfile* wf, int sec ) +void Canvas::Load( Worldfile* wf, int sec ) { this->wf = wf; camera.Load( wf, sec ); @@ -1143,13 +1146,13 @@ if( ! world->paused ) // // start the timer that causes regular redraws Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, + (Fl_Timeout_Handler)Canvas::TimerCallback, this); invalidate(); // we probably changed something } -void StgCanvas::Save( Worldfile* wf, int sec ) +void Canvas::Save( Worldfile* wf, int sec ) { camera.Save( wf, sec ); perspective_camera.Save( wf, sec ); @@ -1175,7 +1178,7 @@ } -void StgCanvas::draw() +void Canvas::draw() { // static unsigned long calls=0; // printf( "Draw calls %lu\n", ++calls ); @@ -1209,7 +1212,7 @@ //Follow the selected robot if( showFollow && last_selection ) { - stg_pose_t gpose = last_selection->GetGlobalPose(); + Pose gpose = last_selection->GetGlobalPose(); if( pCamOn == true ) { perspective_camera.setPose( gpose.x, gpose.y, 0.2 ); @@ -1225,7 +1228,7 @@ renderFrame(); } -void StgCanvas::resize(int X,int Y,int W,int H) +void Canvas::resize(int X,int Y,int W,int H) { Fl_Gl_Window::resize(X,Y,W,H); FixViewport(W,H); Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/gl.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -9,12 +9,12 @@ } // transform the current coordinate frame by the given pose -void Stg::gl_pose_shift( const stg_pose_t &pose ) +void Stg::gl_pose_shift( const Pose &pose ) { gl_coord_shift( pose.x, pose.y, pose.z, pose.a ); } -void Stg::gl_pose_inverse_shift( const stg_pose_t &pose ) +void Stg::gl_pose_inverse_shift( const Pose &pose ) { gl_coord_shift( 0,0,0, -pose.a ); gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 ); Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/main.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -57,9 +57,9 @@ if( optindex > 0 ) { const char* worldfilename = argv[optindex]; - StgWorld* world = ( usegui ? - new StgWorldGui( 400, 300, worldfilename ) : - new StgWorld( worldfilename ) ); + World* world = ( usegui ? + new WorldGui( 400, 300, worldfilename ) : + new World( worldfilename ) ); world->Load( worldfilename ); loaded_world_file = true; } @@ -69,20 +69,20 @@ if( loaded_world_file == false ) { // TODO: special window/loading dialog for this case - new StgWorldGui( 400, 300 ); + new WorldGui( 400, 300 ); } if( usegui == true ) { //don't close the window once time has finished while( true ) - StgWorld::UpdateAll(); + World::UpdateAll(); } else { //close program once time has completed bool quit = false; while( quit == false ) - quit = StgWorld::UpdateAll(); + quit = World::UpdateAll(); } } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-04 18:46:25 UTC (rev 7239) +++ code/stage/trunk/libstage/model.cc 2009-01-05 02:18:00 UTC (rev 7240) @@ -1,100 +1,94 @@ /** @defgroup model -The basic model simulates an object with basic properties; position, -size, velocity, color, visibility to various sensors, etc. The basic -model also has a body made up of a list of lines. Internally, the -basic model is used as the base class for all other model types. You -can use the basic model to simulate environmental objects. + The basic model simulates an object with basic properties; position, + size, velocity, color, visibility to various sensors, etc. The basic + model also has a body made up of a list of lines. Internally, the + basic model is used as the base class for all other model types. You + can use the basic model to simulate environmental objects. -API: Stg::StgModel + API: Stg::Model -<h2>Worldfile properties</h2> + <h2>Worldfile properties</h2> -@par Summary and default values + @par Summary and default values -@verbatim -model -( - pose [ 0.0 0.0 0.0 0.0 ] - size [ 0.1 0.1 0.1 ] - origin [ 0.0 0.0 0.0 0.0 ] - velocity [ 0.0 0.0 0.0 0.0 ] + @verbatim + model + ( + pose [ 0.0 0.0 0.0 0.0 ] + size [ 0.1 0.1 0.1 ] + origin [ 0.0 0.0 0.0 0.0 ] + velocity [ 0.0 0.0 0.0 0.0 ] - color "red" - color_rgba [ 0.0 0.0 0.0 1.0 ] - bitmap "" - ctrl "" + color "red" + color_rgba [ 0.0 0.0 0.0 1.0 ] + bitmap "" + ctrl "" - # determine how the model appears in various sensors - fiducial_return 0 - fiducial_key 0 - obstacle_return 1 - ranger_return 1 - blob_return 1 - laser_return LaserVisible - gripper_return 0 + # determine how the model appears in various sensors + fiducial_return 0 + fiducial_key 0 + obstacle_return 1 + ranger_return 1 + blob_return 1 + laser_return LaserVisible + gripper_return 0 - # GUI properties - gui_nose 0 - gui_grid 0 - gui_outline 1 - gui_movemask <0 if top level or (STG_MOVE_TRANS | STG_MOVE_ROT)>; + # GUI properties + gui_nose 0 + gui_grid 0 + gui_outline 1 + gui_movemask <0 if top level or (STG_MOVE_TRANS | STG_MOVE_ROT)>; - blocks 0 - block[0].points 0 - block[0].point[0] [ 0.0 0.0 ] - block[0].z [ 0.0 1.0 ] - block[0].color "<color>" + boundary 0 + mass 10.0 + map_resolution 0.1 + say "" + alwayson 0 + ) + @endverbatim - boundary 0 - mass 10.0 - map_resolution 0.1 - say "" - alwayson 0 -) -@endverbatim - -@par Details + @par Details -- pose [ x:<float> y:<float> z:<float> heading:<float> ] \n - specify the pose of the model in its parent's coordinate system -- size [ x:<float> y:<float> z:<float> ]\n - specify the size of the model in each dimension -- origin [ x:<float> y:<float> z:<float> heading:<float> ]\n - specify the position of the object's center, relative to its pose -- velocity [ x:<float> y:<float> z:<float> heading:<float> omega:<float> ]\n - Specify the initial velocity of the model. Note that if the model hits an obstacle, its velocity will be set to zero. + - pose [ x:<float> y:<float> z:<float> heading:<float> ] \n + specify the pose of the model in its parent's coordinate system + - size [ x:<float> y:<float> z:<float> ]\n + specify the size of the model in each dimension + - origin [ x:<float> y:<float> z:<float> heading:<float> ]\n + specify the position of the object's center, relative to its pose + - velocity [ x:<float> y:<float> z:<float> heading:<float> omega:<float> ]\n + Specify the initial velocity of the model. Note that if the model hits an obstacle, its velocity will be set to zero. -- color <string>\n - specify the color of the object using a color name from the X11 database (rgb.txt) -- bitmap filename:<string>\n - Draw the model by interpreting the lines in a bitmap (bmp, jpeg, gif, png supported). The file is opened and parsed into a set of lines. The lines are scaled to fit inside the rectangle defined by the model's current size. -- ctrl <string>\n - specify the controller module for the model + - color <string>\n + specify the color of the object using a color name from the X11 database (rgb.txt) + - bitmap filename:<string>\n + Draw the model by interpreting the lines in a bitmap (bmp, jpeg, gif, png supported). The file is opened and parsed into a set of lines. The lines are scaled to fit inside the rectangle defined by the model's current size. + - ctrl <string>\n + specify the controller module for the model -- fiducial_return fiducial_id:<int>\n - if non-zero, this model is detected by fiducialfinder sensors. The value is used as the fiducial ID. -- fiducial_key <int> - models are only detected by fiducialfinders if the fiducial_key values of model and fiducialfinder match. This allows you to have several independent types of fiducial in the same environment, each type only showing up in fiducialfinders that are "tuned" for it. -- obstacle_return <int>\n - if 1, this model can collide with other models that have this property set -- ranger_return <int>\n - if 1, this model can be detected by ranger sensors -- blob_return <int>\n - if 1, this model can be detected in the blob_finder (depending on its color) -- laser_return <int>\n - if 0, this model is not detected by laser sensors. if 1, the model shows up in a laser sensor with normal (0) reflectance. If 2, it shows up with high (1) reflectance. -- gripper_return <int>\n - iff 1, this model can be gripped by a gripper and can be pushed around by collisions with anything that has a non-zero obstacle_return. + - fiducial_return fiducial_id:<int>\n + if non-zero, this model is detected by fiducialfinder sensors. The value is used as the fiducial ID. + - fiducial_key <int> + models are only detected by fiducialfinders if the fiducial_key values of model and fiducialfinder match. This allows you to have several independent types of fiducial in the same environment, each type only showing up in fiducialfinders that are "tuned" for it. + - obstacle_return <int>\n + if 1, this model can collide with other models that have this property set + - ranger_return <int>\n + if 1, this model can be detected by ranger sensors + - blob_return <int>\n + if 1, this model can be detected in the blob_finder (depending on its color) + - laser_return <int>\n + if 0, this model is not detected by laser sensors. if 1, the model shows up in a laser sensor with normal (0) reflectance. If 2, it shows up with high (1) reflectance. + - gripper_return <int>\n + iff 1, this model can be gripped by a gripper and can be pushed around by collisions with anything that has a non-zero obstacle_return. -- gui_nose <int>\n - if 1, draw a nose on the model showing its heading (positive X axis) -- gui_grid <int>\n - if 1, draw a scaling grid over the model -- gui_outline <int>\n - if 1, draw a bounding box around the model, indicating its size -- gui_movemask <int>\n - define how the model can be moved by the mouse in the GUI window + - gui_nose <int>\n + if 1, draw a nose on the model showing its heading (positive X axis) + - gui_grid <int>\n + if 1, draw a scaling grid over the model + - gui_outline <int>\n + if 1, draw a bounding box around the model, indicating its size + - gui_movemask <int>\n + define how the model can be moved by the mouse in the GUI window */ @@ -124,76 +118,60 @@ static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black // static members -uint32_t StgModel::count = 0; -GHashTable* StgModel::modelsbyid = g_hash_table_new( NULL, NULL ); +uint32_t Model::count = 0; +GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); // constructor -StgModel::StgModel( StgWorld* world, - StgModel* parent, - const stg_model_type_t type ) - : StgAncestor(), - access_mutex(NULL), - blinkenlights( g_ptr_array_new() ), - blob_return(true), - blockgroup(), - blocks_dl(0), - boundary(false), - callbacks( g_hash_table_new( g_int_hash, g_int_equal ) ), - color( 0xFFFF0000 ), // red - data_fresh(false), - disabled(false), - fiducial_key(0), - fiducial_return(0), - flag_list(NULL), - geom(), - gripper_return(false), - gui_grid(false), - gui_mask( parent ? 0 : (STG_MOVE_TRANS | STG_MOVE_ROT) ), - gui_nose(false), - gui_outline(false), - has_default_block( true ), - hook_load(0), - hook_save(0), - hook_shutdown(0), - hook_startup(0), - hook_update(0), - id( StgModel::count++ ), - initfunc(NULL), - interval((stg_usec_t)1e4), // 10msec - laser_return(LaserVisible), - last_update(0), - map_caches_are_invalid( true ), - map_resolution(0.1), - mass(0), - obstacle_return(true), - on_update_list( false ), - on_velocity_list( false ), - parent(parent), - pose(), - props(NULL), - ranger_return(true), - rebuild_displaylist(true), - say_string(NULL), - stall(false), - subs(0), - thread_safe( false ), - trail( g_array_new( false, false, sizeof(stg_trail_item_t) )), - type(type), - used(false), - velocity(), - watts(0), - wf(NULL), - wf_entity(0), - world(world) +Model::Model( World* world, + Model* parent, + const stg_model_type_t type ) + : Ancestor(), + access_mutex(NULL), + blinkenlights( g_ptr_array_new() ), + blockgroup(), + blocks_dl(0), + boundary(false), + callbacks( g_hash_table_new( g_int_hash, g_int_equal ) ), + color( 0xFFFF0000 ), // red + data_fresh(false), + disabled(false), + flag_list(NULL), + geom(), + has_default_block( true ), + id( Model::count++ ), + initfunc(NULL), + interval((stg_usec_t)1e4), // 10msec + last_update(0), + map_caches_are_invalid( true ), + map_resolution(0.1), + mass(0), + on_update_list( false ), + on_velocity_list( false ), + parent(parent), + pose(), + props(NULL), + rebuild_displaylist(true), + say_string(NULL), + stall(false), + subs(0), + thread_safe( false ), + trail( g_array_new( false, false, sizeof(stg_trail_item_t) )), + type(type), + used(false), + velocity(), + watts(0), + wf(NULL), + wf_entity(0), + world(world) { assert( modelsbyid ); assert( world ); PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %d ", - world->Token(), - parent ? parent->Token() : "(null)", - type ); + world->Token(), + parent ? parent->Token() : "(null)", + type ); g_hash_table_insert( modelsbyid, (void*)id, this ); @@ -202,8 +180,12 @@ if ( parent ) parent->AddChild( this ); else - world->AddChild( this ); - + { + world->AddChild( this ); + // top level models are draggable in the GUI + gui.mask = (STG_MOVE_TRANS | STG_MOVE_ROT); + } + world->AddModel( this ); g_datalist_init( &this->props ); @@ -211,11 +193,10 @@ // now we can add the basic square shape AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 ); - PRINT_DEBUG2( "finished model %s @ %p", - this->token, this ); + PRINT_DEBUG2( "finished model %s @ %p", this->token, this ); } -StgModel::~StgModel( void ) +Model::~Model( void ) { UnMap(); // remove from the raytrace bitmap @@ -231,21 +212,21 @@ g_datalist_clear( &props ); - g_hash_table_remove( StgModel::modelsbyid, (void*)id ); + g_hash_table_remove( Model::modelsbyid, (void*)id ); world->RemoveModel( this ); } -void StgModel::StartUpdating() +void Model::StartUpdating() { if( ! on_update_list ) - { - on_update_list = true; - world->StartUpdatingModel( this ); - } + { + on_update_list = true; + world->StartUpdatingModel( this ); + } } -void StgModel::StopUpdating() +void Model::StopUpdating() { on_update_list = false; world->StopUpdatingModel( this ); @@ -254,7 +235,7 @@ // this should be called after all models have loaded from the // worldfile - it's a chance to do any setup now that all models are // in existence -void StgModel::Init() +void Model::Init() { // init is called after the model is loaded blockgroup.CalcSize(); @@ -267,44 +248,44 @@ Subscribe(); } -void StgModel::InitRecursive() +void Model::InitRecursive() { // init children first - LISTMETHOD( children, StgModel*, InitRecursive ); + LISTMETHOD( children, Model*, InitRecursive ); Init(); } -void StgModel::AddFlag( StgFlag* flag ) +void Model::AddFlag( Flag* flag ) { if( flag ) flag_list = g_list_append( this->flag_list, flag ); } -void StgModel::RemoveFlag( StgFlag* flag ) +void Model::RemoveFlag( Flag* flag ) { if( flag ) flag_list = g_list_remove( this->flag_list, flag ); } -void StgModel::PushFlag( StgFlag* flag ) +void Model::PushFlag( Flag* flag ) { if( flag ) flag_list = g_list_prepend( flag_list, flag); } -StgFlag* StgModel::PopFlag() +Flag* Model::PopFlag() { if( flag_list == NULL ) return NULL; - StgFlag* flag = (StgFlag*)flag_list->data; + Flag* flag = (Flag*)flag_list->data; flag_list = g_list_remove( flag_list, flag ); return flag; } -void StgModel::ClearBlocks( void ) +void Model::ClearBlocks( void ) { UnMap(); blockgroup.Clear(); @@ -314,23 +295,23 @@ NeedRedraw(); } -void StgModel::LoadBlock( Worldfile* wf, int entity ) +void Model::LoadBlock( Worldfile* wf, int entity ) { if( has_default_block ) - { - blockgroup.Clear(); - has_default_block = false; - } + { + blockgroup.Clear(); + has_default_block = false; + } blockgroup.LoadBlock( this, wf, entity ); } -void StgModel::AddBlockRect( stg_meters_t x, - stg_meters_t y, - stg_meters_t dx, - stg_meters_t dy, - stg_meters_t dz ) +void Model::AddBlockRect( stg_meters_t x, + stg_meters_t y, + stg_meters_t dx, + stg_meters_t dy, + stg_meters_t dz ) { UnMap(); @@ -344,69 +325,69 @@ pts[3].x = x; pts[3].y = y + dy; - blockgroup.AppendBlock( new StgBlock( this, - pts, 4, - 0, dz, - color, - true ) ); + blockgroup.AppendBlock( new Block( this, + pts, 4, + 0, dz, + color, + true ) ); } -stg_raytrace_result_t StgModel::Raytrace( const stg_pose_t &pose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest ) +stg_raytrace_result_t Model::Raytrace( const Pose &pose, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest ) { return world->Raytrace( LocalToGlobal(pose), - range, - func, - this, - arg, - ztest ); + range, + func, + this, + arg, + ztest ); } -stg_raytrace_result_t StgModel::Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest ) +stg_raytrace_result_t Model::Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest ) { - stg_pose_t raystart; + Pose raystart; bzero( &raystart, sizeof(raystart)); raystart.a = bearing; return world->Raytrace( LocalToGlobal(raystart), - range, - func, - this, - arg, - ztest ); + range, + func, + this, + arg, + ztest ); } -void StgModel::Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest ) +void Model::Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest ) { - stg_pose_t raystart; + Pose raystart; bzero( &raystart, sizeof(raystart)); raystart.a = bearing; world->Raytrace( LocalToGlobal(raystart), - range, - fov, - func, - this, - arg, - samples, - sample_count, - ztest ); + range, + fov, + func, + this, + arg, + samples, + sample_count, + ztest ); } // utility for g_free()ing everything in a list @@ -420,10 +401,10 @@ } // convert a global pose into the model's local coordinate system -stg_pose_t StgModel::GlobalToLocal( stg_pose_t pose ) +Pose Model::GlobalToLocal( Pose pose ) { // get model's global pose - stg_pose_t org = GetGlobalPose(); + Pose org = GetGlobalPose(); // compute global pose in local coords double sx = (pose.x - org.x) * cos(org.a) + (pose.y - org.y) * sin(org.a); @@ -440,7 +421,7 @@ } -void StgModel::Say( const char* str ) +void Model::Say( const char* str ) { if( say_string ) free( say_string ); @@ -448,29 +429,29 @@ } // returns true iff model [testmod] is an antecedent of this model -bool StgModel::IsAntecedent( StgModel* testmod ) +bool Model::IsAntecedent( Model* testmod ) { if( parent == NULL ) - return false; + return false; if( parent == testmod ) - return true; + return true; return parent->IsAntecedent( testmod ); } // returns true iff model [testmod] is a descendent of this model -bool StgModel::IsDescendent( StgModel* testmod ) +bool Model::IsDescendent( Model* testmod ) { for( GList* it=this->children; it; it=it->next ) { - StgModel* child = (StgModel*)it->data; + Model* child = (Model*)it->data; - if( child == testmod ) - return true; + if( child == testmod ) + return true; - if( child->IsDescendent( testmod ) ) - return true; + if( child->IsDescendent( testmod ) ) + return true; } // neither mod nor a child of this matches testmod @@ -478,20 +459,20 @@ } // returns true iff model [mod1] and [mod2] are in the same model tree -bool StgModel::IsRelated( StgModel* mod2 ) +bool Model::IsRelated( Model* mod2 ) { return( (this == mod2) || IsAntecedent( mod2 ) || IsDescendent( mod2 ) ); } // get the model's velocity in the global frame -stg_velocity_t StgModel::GetGlobalVelocity() +Velocity Model::GetGlobalVelocity() { - stg_pose_t gpose = GetGlobalPose(); + Pose gpose = GetGlobalPose(); double cosa = cos( gpose.a ); double sina = sin( gpose.a ); - stg_velocity_t gv; + Velocity gv; gv.x = velocity.x * cosa - velocity.y * sina; gv.y = velocity.x * sina + velocity.y * cosa; gv.a = velocity.a; @@ -504,14 +485,14 @@ } // set the model's velocity in the global frame -void StgModel::SetGlobalVelocity( stg_velocity_t gv ) +void Model::SetGlobalVelocity( Velocity gv ) { - stg_pose_t gpose = GetGlobalPose(); + Pose... [truncated message content] |
From: <rt...@us...> - 2009-01-05 03:24:08
|
Revision: 7243 http://playerstage.svn.sourceforge.net/playerstage/?rev=7243&view=rev Author: rtv Date: 2009-01-05 03:24:04 +0000 (Mon, 05 Jan 2009) Log Message: ----------- applied Geoff Bigg's CMake patch to resolve FLTK link issues on various platforms. Thanks Geoff. Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstageplugin/CMakeLists.txt code/stage/trunk/worlds/benchmark/CMakeLists.txt Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-01-05 03:07:38 UTC (rev 7242) +++ code/stage/trunk/CMakeLists.txt 2009-01-05 03:24:04 UTC (rev 7243) @@ -93,6 +93,16 @@ MESSAGE( FATAL_ERROR "Cannot find fltk-config, aborting" ) ENDIF( NOT FLTK_CONFIG) + +OPTION (LINK_FLTK_STATICALLY "Link to the FLTK libraries statically" OFF) +IF (LINK_FLTK_STATICALLY) + SET (FLTK_CONFIG_CMD "--ldstaticflags") + MESSAGE (STATUS "Linking to FLTK statically.") +ELSE (LINK_FLTK_STATICALLY) + SET (FLTK_CONFIG_CMD "--ldflags") + MESSAGE (STATUS "Linking to FLTK dynamically.") +ENDIF (LINK_FLTK_STATICALLY) + EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --version OUTPUT_VARIABLE FLTK_VERSION OUTPUT_STRIP_TRAILING_WHITESPACE ) @@ -100,7 +110,7 @@ EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images --cxxflags OUTPUT_VARIABLE FLTK_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE ) -EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images --ldstaticflags +EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images ${FLTK_CONFIG_CMD} OUTPUT_VARIABLE FLTK_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE ) EXECUTE_PROCESS ( COMMAND ${FLTK_CONFIG} --use-gl --use-images --libs Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-05 03:07:38 UTC (rev 7242) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-01-05 03:24:04 UTC (rev 7243) @@ -10,9 +10,11 @@ # create a library module for each plugin and link libstage to each + foreach( PLUGIN ${PLUGINS} ) ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc ) TARGET_LINK_LIBRARIES( ${PLUGIN} stage ) + set_source_files_properties( ${PLUGIN}.cc PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) endforeach( PLUGIN ) # delete the "lib" prefix from the plugin libraries Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-05 03:07:38 UTC (rev 7242) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-05 03:24:04 UTC (rev 7243) @@ -71,6 +71,8 @@ add_executable( stagetest ${stagetestSrcs} ) target_link_libraries( stagetest stage ) +set_target_properties( stagetest PROPERTIES LINK_FLAGS "${FLTK_LDFLAGS}" ) +set_source_files_properties( ${stagetestSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) INSTALL(TARGETS stagebinary stage Modified: code/stage/trunk/libstageplugin/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/CMakeLists.txt 2009-01-05 03:07:38 UTC (rev 7242) +++ code/stage/trunk/libstageplugin/CMakeLists.txt 2009-01-05 03:24:04 UTC (rev 7243) @@ -28,6 +28,8 @@ ) +set_source_files_properties( ${stagepluginSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) + IF (BUILD_LSPTEST) ADD_SUBDIRECTORY(test) ENDIF (BUILD_LSPTEST) Modified: code/stage/trunk/worlds/benchmark/CMakeLists.txt =================================================================== --- code/stage/trunk/worlds/benchmark/CMakeLists.txt 2009-01-05 03:07:38 UTC (rev 7242) +++ code/stage/trunk/worlds/benchmark/CMakeLists.txt 2009-01-05 03:24:04 UTC (rev 7243) @@ -1,5 +1,6 @@ SET( expandSrcs expand.cc ) ADD_LIBRARY( expand MODULE ${expandSrcs} ) TARGET_LINK_LIBRARIES( expand stage ) +set_source_files_properties( ${expandSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) SET_TARGET_PROPERTIES( expand PROPERTIES PREFIX "" ) INSTALL( TARGETS expand DESTINATION lib) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-18 00:10:27
|
Revision: 7279 http://playerstage.svn.sourceforge.net/playerstage/?rev=7279&view=rev Author: rtv Date: 2009-01-18 00:10:21 +0000 (Sun, 18 Jan 2009) Log Message: ----------- added simple power model and some cosmetic and gui fixes Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/map.inc code/stage/trunk/worlds/pioneer.inc Added Paths: ----------- code/stage/trunk/libstage/powerpack.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-18 00:10:21 UTC (rev 7279) @@ -27,6 +27,7 @@ option.hh options_dlg.cc options_dlg.hh + powerpack.cc region.cc resource.cc stage.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -151,6 +151,7 @@ on_velocity_list( false ), parent(parent), pose(), + power_pack( NULL ), props(NULL), rebuild_displaylist(true), say_string(NULL), @@ -656,7 +657,26 @@ { // printf( "[%llu] %s update (%d subs)\n", // this->world->sim_time, this->token, this->subs ); + + // f we're drawing current and a power pack has been installed + if( power_pack && (watts > 0) ) + { + // consume energy stored in the power pack + stg_joules_t consumed = watts * (world->interval_sim * 1e-6); + power_pack->stored -= consumed; + /* + printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", + token, + watts, + consumed, + power_pack, + power_pack->stored, + power_pack->capacity, + power_pack->stored / power_pack->capacity * 100.0 ); + */ + } + CallCallbacks( &hooks.update ); last_update = world->sim_time; } @@ -920,18 +940,19 @@ void Model::DrawStatus( Camera* cam ) { - if( say_string ) + + + if( say_string || power_pack ) { float yaw, pitch; pitch = - cam->pitch(); yaw = - cam->yaw(); - float robotAngle = -rtod(pose.a); + Pose gpz = GetGlobalPose(); + + float robotAngle = -rtod(gpz.a); glPushMatrix(); - fl_font( FL_HELVETICA, 12 ); - float w = gl_width( this->say_string ); // scaled text width - float h = gl_height(); // scaled text height // move above the robot glTranslatef( 0, 0, 0.5 ); @@ -939,61 +960,77 @@ // rotate to face screen glRotatef( robotAngle - yaw, 0,0,1 ); glRotatef( -pitch, 1,0,0 ); + + + //if( ! parent ) + // glRectf( 0,0,1,1 ); - //get raster positition, add gl_width, then project back to world coords - glRasterPos3f( 0, 0, 0 ); - GLfloat pos[ 4 ]; - glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); - - GLboolean valid; - glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - if( valid ) - { - GLdouble wx, wy, wz; - GLint viewport[4]; - glGetIntegerv(GL_VIEWPORT, viewport); + if( power_pack && (power_pack->mod == this) ) + power_pack->Visualize( cam ); + + if( say_string ) + { + //get raster positition, add gl_width, then project back to world coords + glRasterPos3f( 0, 0, 0 ); + GLfloat pos[ 4 ]; + glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); - GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + GLboolean valid; + glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - GLdouble projection[16]; - glGetDoublev(GL_PROJECTION_MATRIX, projection); - - //get width and height in world coords - gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - w = wx; - gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - h = wy; - - // calculate speech bubble margin - const float m = h/10; - - // draw inside of bubble - PushColor( BUBBLE_FILL ); - glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); - glEnable( GL_POLYGON_OFFSET_FILL ); - glPolygonOffset( 1.0, 1.0 ); - gl_draw_octagon( w, h, m ); - glDisable( GL_POLYGON_OFFSET_FILL ); - PopColor(); - - // draw outline of bubble - PushColor( BUBBLE_BORDER ); - glLineWidth( 1 ); - glEnable( GL_LINE_SMOOTH ); - glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); - glPopAttrib(); - PopColor(); - - PushColor( BUBBLE_TEXT ); - // draw text inside the bubble - gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); - PopColor(); - } - glPopMatrix(); - } + if( valid ) + { + + fl_font( FL_HELVETICA, 12 ); + float w = gl_width( this->say_string ); // scaled text width + float h = gl_height(); // scaled text height + + GLdouble wx, wy, wz; + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + + GLdouble modelview[16]; + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + + GLdouble projection[16]; + glGetDoublev(GL_PROJECTION_MATRIX, projection); + + //get width and height in world coords + gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + w = wx; + gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + h = wy; + + // calculate speech bubble margin + const float m = h/10; + + // draw inside of bubble + PushColor( BUBBLE_FILL ); + glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); + glPolygonMode( GL_FRONT, GL_FILL ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 1.0, 1.0 ); + gl_draw_octagon( w, h, m ); + glDisable( GL_POLYGON_OFFSET_FILL ); + PopColor(); + + // draw outline of bubble + PushColor( BUBBLE_BORDER ); + glLineWidth( 1 ); + glEnable( GL_LINE_SMOOTH ); + glPolygonMode( GL_FRONT, GL_LINE ); + gl_draw_octagon( w, h, m ); + glPopAttrib(); + PopColor(); + + PushColor( BUBBLE_TEXT ); + // draw text inside the bubble + gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + PopColor(); + } + } + glPopMatrix(); + } if( stall ) { @@ -1074,9 +1111,13 @@ PushColor( flag->color ); + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); gluQuadricDrawStyle( quadric, GLU_FILL ); gluSphere( quadric, flag->size/2.0, 4,2 ); - + glDisable(GL_POLYGON_OFFSET_FILL); + // draw the edges darker version of the same color double r,g,b,a; stg_color_unpack( flag->color, &r, &g, &b, &a ); @@ -1149,7 +1190,20 @@ void Model::DataVisualize( Camera* cam ) { - // do nothing +// if( power_pack ) +// { +// // back into global coords to get rid of my rotation +// glPushMatrix(); +// gl_pose_inverse_shift( GetGlobalPose() ); + +// // shift to the top left corner of the model (roughly) +// glTranslatef( pose.x - geom.size.x/2.0, +// pose.y + geom.size.y/2.0, +// pose.z + geom.size.z ); + +// power_pack->Visualize( cam ); +// glPopMatrix(); +// } } void Model::DataVisualizeTree( Camera* cam ) Modified: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model.hh 2009-01-18 00:10:21 UTC (rev 7279) @@ -1,6 +1,30 @@ #ifndef MODEL_H #define MODEL_H +class Camera; + +/** energy data packet */ +class PowerPack +{ +public: + PowerPack( Model* mod ); + + /** The model that owns this object */ + Model* mod; + + /** Energy stored */ + stg_joules_t stored; + + /** Energy capacity */ + stg_joules_t capacity; + + /** TRUE iff the device is receiving energy from a charger */ + bool charging; + + /** OpenGL visualization of the powerpack state */ + void Visualize( Camera* cam ); +}; + class Visibility { public: @@ -76,7 +100,7 @@ nose = wf->ReadInt( wf_entity, "gui_nose", nose); grid = wf->ReadInt( wf_entity, "gui_grid", grid); outline = wf->ReadInt( wf_entity, "gui_outline", outline); - mask = wf->ReadInt( wf_entity, "gui_mask", mask); + mask = wf->ReadInt( wf_entity, "gui_movemask", mask); } }; @@ -129,7 +153,6 @@ Geom geom; Pose global_pose; bool gpose_dirty; //< set this to indicate that global pose may have changed - /** Controls our appearance and functionality in the GUI, if used */ GuiState gui; @@ -157,6 +180,9 @@ global coordinate frame is the parent is NULL. */ Pose pose; + /** Optional attached PowerPack, defaults to NULL */ + PowerPack* power_pack; + /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code to associate arbitrary items with a model. */ @@ -699,47 +725,9 @@ void RemoveAllColors(); }; -// ENERGY model -------------------------------------------------------------- -/** energy data packet */ -typedef struct -{ - /** estimate of current energy stored */ - stg_joules_t stored; - /** TRUE iff the device is receiving energy from a charger */ - stg_bool_t charging; - /** diatance to charging device */ - stg_meters_t range; - - /** an array of pointers to connected models */ - GPtrArray* connections; -} stg_energy_data_t; - -/** energy config packet (use this to set or get energy configuration)*/ -typedef struct -{ - /** maximum storage capacity */ - stg_joules_t capacity; - - /** When charging another device, supply this many Joules/sec at most*/ - stg_watts_t give_rate; - - /** When charging from another device, receive this many Joules/sec at most*/ - stg_watts_t take_rate; - - /** length of the charging probe */ - stg_meters_t probe_range; - - /** iff TRUE, this device will supply power to connected devices */ - stg_bool_t give; - -} stg_energy_config_t; - -// there is currently no energy command packet - - // LASER MODEL -------------------------------------------------------- /** laser sample packet Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model_load.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -19,7 +19,59 @@ PRINT_DEBUG1( "Model \"%s\" loading...", token ); + if( wf->PropertyExists( wf_entity, "joules" ) ) + { + if( !power_pack ) + power_pack = new PowerPack( this ); + + power_pack->stored = + wf->ReadFloat( wf_entity, "joules", power_pack->stored ); + /* assume that the store is full, so the capacity is the same as + the charge */ + power_pack->capacity = power_pack->stored; + } + + if( wf->PropertyExists( wf_entity, "joules_capacity" ) ) + { + if( !power_pack ) + power_pack = new PowerPack( this ); + + power_pack->capacity = + wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity ); + + } + + /** if the capacity has been specified, limit the store to the capacity */ + if( power_pack && (power_pack->stored > power_pack->capacity) ) + { + power_pack->stored = power_pack->capacity; + PRINT_WARN3( "model %s energy storage exceeds capacity (%.2f / %.2f joules). Limited stored energy to max capactity.", + token, + power_pack->stored, + power_pack->capacity ); + } + + if( wf->PropertyExists( wf_entity, "watts" ) ) + { + watts = wf->ReadFloat( wf_entity, "watts", watts ); + + if( watts > 0 ) + { + // find a power pack attached to me or an ancestor in my tree + while( (!power_pack) && parent ) + { + power_pack = parent->power_pack; + } + + if( power_pack == NULL ) + { + PRINT_WARN2( "worldfile requests %.2f watts for model %s, but can not find an energy source. Setting watts has no effect unless you also specify a \"joules\" value for this model or an ancestor.", watts, token ); + exit(-1); + } + } + } + if( wf->PropertyExists( wf_entity, "debug" ) ) { PRINT_WARN2( "debug property specified for model %d %s\n", Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model_position.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -69,8 +69,8 @@ -const double STG_POSITION_WATTS_KGMS = 5.0; // cost per kg per meter per second -const double STG_POSITION_WATTS = 2.0; // base cost of position device +const double STG_POSITION_WATTS_KGMS = 10.0; // current per kg per meter per second +const double STG_POSITION_WATTS = 1.0; // base cost of position device // simple odometry error model parameters. the error is selected at // random in the interval -MAX/2 to +MAX/2 at startup @@ -385,13 +385,13 @@ default: PRINT_ERR1( "unrecognized position command mode %d", control_mode ); } - + // simple model of power consumption - // this->watts = STG_POSITION_WATTS + - //fabs(vel->x) * STG_POSITION_WATTS_KGMS * this->mass + - //fabs(vel->y) * STG_POSITION_WATTS_KGMS * this->mass + - //fabs(vel->a) * STG_POSITION_WATTS_KGMS * this->mass; - + watts = STG_POSITION_WATTS + + fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; + //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", // this->token, // this->velocity.x, @@ -636,6 +636,9 @@ glPopMatrix(); } + + // inherit more viz + Model::DataVisualize( cam ); } void ModelPosition::DrawWaypoints() Added: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc (rev 0) +++ code/stage/trunk/libstage/powerpack.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -0,0 +1,58 @@ +/** powerpack.cc + Simple model of energy storage + Richard Vaughan + Created 2009.1.15 + $Id$ +*/ + +#include "stage_internal.hh" + +PowerPack::PowerPack( Model* mod ) : + mod( mod), stored( 0.0 ), capacity( 0.0 ) +{ + // nothing to do +}; + + +/** OpenGL visualization of the powerpack state */ +void PowerPack::Visualize( Camera* cam ) +{ + const double height = 0.5; + const double width = 0.3; + + double percent = stored/capacity * 100.0; + + if( percent > 50 ) + glColor4f( 0,1,0, 0.7 ); // green + else if( percent > 25 ) + glColor4f( 1,0,1, 0.7 ); // magenta + else + glColor4f( 1,0,0, 0.7 ); // red + + static char buf[6]; + snprintf( buf, 6, "%.0f", percent ); + + glTranslatef( -width, 0.0, 0.0 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + GLfloat fullness = height * (percent * 0.01); + glRectf( 0,0,width, fullness); + + // outline the charge-o-meter + glTranslatef( 0,0,0.001 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glColor4f( 0,0,0,0.7 ); + glRectf( 0,0,width, height ); + + glBegin( GL_LINES ); + glVertex2f( 0, fullness ); + glVertex2f( width, fullness ); + glEnd(); + + // draw the percentage + //gl_draw_string( -0.2, 0, 0, buf ); + + // ? + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/stage.hh 2009-01-18 00:10:21 UTC (rev 7279) @@ -847,7 +847,7 @@ stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI - stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps + stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps GList* ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in ms GHashTable* superregions; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/fasr.world 2009-01-18 00:10:21 UTC (rev 7279) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 00 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 @@ -22,7 +22,7 @@ ( size [ 600.000 600.000 ] - center [ -0.118 -0.212 ] + center [ -0.521 -0.218 ] rotate [ 0 0 ] scale 30.883 @@ -61,13 +61,20 @@ ctrl "sink" ) +#charger +#( +# pose +# +#) + define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 ) + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" + joules 1000000 ) -autorob( pose [5.099 4.804 0 -73.937] ) +autorob( pose [4.116 6.107 0 -147.323] ) autorob( pose [6.471 5.304 0 14.941] ) autorob( pose [5.937 4.858 0 -147.503] ) autorob( pose [7.574 6.269 0 -111.715] ) Modified: code/stage/trunk/worlds/map.inc =================================================================== --- code/stage/trunk/worlds/map.inc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/map.inc 2009-01-18 00:10:21 UTC (rev 7279) @@ -24,6 +24,11 @@ color "orange" size [ 2 2 0.02 ] + gui_nose 0 + gui_grid 0 + gui_movemask 0 + gui_outline 0 + # insensible to collision and range sensors obstacle_return 0 laser_return 0 Modified: code/stage/trunk/worlds/pioneer.inc =================================================================== --- code/stage/trunk/worlds/pioneer.inc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/pioneer.inc 2009-01-18 00:10:21 UTC (rev 7279) @@ -127,7 +127,7 @@ gui_nose 1 # estimated mass in KG - mass 15.0 + mass 23.0 # use the sonar array defined above with a small vertical offset to # drop the sensors into the robot body This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-18 03:12:11
|
Revision: 7280 http://playerstage.svn.sourceforge.net/playerstage/?rev=7280&view=rev Author: rtv Date: 2009-01-18 01:56:16 +0000 (Sun, 18 Jan 2009) Log Message: ----------- added simple charger and visualizations Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Added Paths: ----------- code/stage/trunk/libstage/charger.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-18 01:56:16 UTC (rev 7280) @@ -8,6 +8,7 @@ blockgroup.cc camera.cc canvas.cc + charger.cc file_manager.cc file_manager.hh gl.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/canvas.cc 2009-01-18 01:56:16 UTC (rev 7280) @@ -898,7 +898,9 @@ for( GList* it=selected_models; it; it=it->next ) ((Model*)it->data)->DrawSelected(); - + for( GList* it=world->chargers; it; it=it->next ) + ((Charger*)it->data)->Visualize(); + // useful debug - puts a point at the origin of each model //for( GList* it = world->World::children; it; it=it->next ) // ((Model*)it->data)->DrawOriginTree(); Added: code/stage/trunk/libstage/charger.cc =================================================================== --- code/stage/trunk/libstage/charger.cc (rev 0) +++ code/stage/trunk/libstage/charger.cc 2009-01-18 01:56:16 UTC (rev 7280) @@ -0,0 +1,115 @@ +/** charger.cc + Simple model of energy storage + Richard Vaughan + Created 2009.1.15 + SVN: $Id: stage.hh 7279 2009-01-18 00:10:21Z rtv $ +*/ + + +#include "stage_internal.hh" + +Charger::Charger( World* world ) + : world( world ), watts( 1000.0 ) +{ + printf( "Charger constructed" ); +} + +void Charger::ChargeIfContained( PowerPack* pp, Pose pose ) +{ + if( Contains( pose ) ) + Charge( pp ); +} + +bool Charger::Contains( Pose pose ) +{ + return( pose.x >= volume.x.min && + pose.x < volume.x.max && + pose.y >= volume.y.min && + pose.y < volume.y.max && + pose.z >= volume.z.min && + pose.z < volume.z.max ); +} + +void Charger::Charge( PowerPack* pp ) +{ + double given = watts * world->interval_sim * 1e-6; + + pp->stored += given; + + // do not exceed capacity + pp->stored = MIN( pp->stored, pp->capacity ); + pp->charging = true; + /* + printf( "charger %p at [%.2f %.2f] [%.2f %.2f] [%.2f %.2f] gave pack %p %.3f joules\n", + this, + volume.x.min, + volume.x.max, + volume.y.min, + volume.y.max, + volume.z.min, + volume.z.max, + pp, given ); + + pp->Print( "just charged" ); + */ +} + +void Charger::Visualize() +{ + + glPushMatrix(); + glPolygonMode( GL_FRONT, GL_FILL ); + glColor4f( 1, 0.5,0,0.4 ); + glTranslatef( 0,0,volume.z.min); + glRectf( volume.x.min, volume.y.min, volume.x.max, volume.y.max ); + + glTranslatef( 0,0,volume.z.max ); + glRectf( volume.x.min, volume.y.min, volume.x.max, volume.y.max ); + glPopMatrix(); + + glPushMatrix(); + glPolygonMode( GL_FRONT, GL_LINE ); + glColor4f( 1, 0.5,0,0.8 ); + glTranslatef( 0,0,volume.z.min); + glRectf( volume.x.min, volume.y.min, volume.x.max, volume.y.max ); + + glTranslatef( 0,0,volume.z.max ); + glRectf( volume.x.min, volume.y.min, volume.x.max, volume.y.max ); + glPopMatrix(); + + // ? + glPolygonMode( GL_FRONT, GL_FILL ); +} + +void swap( double &a, double &b ) +{ + double keep = a; + a = b; + b = keep; +} + +void Charger::Load( Worldfile* wf, int entity ) +{ + if( wf->PropertyExists( entity, "volume" ) ) + { + volume.x.min = wf->ReadTupleLength( entity, "volume", 0, volume.x.min ); + volume.x.max = wf->ReadTupleLength( entity, "volume", 1, volume.x.max ); + volume.y.min = wf->ReadTupleLength( entity, "volume", 2, volume.y.min ); + volume.y.max = wf->ReadTupleLength( entity, "volume", 3, volume.y.max ); + volume.z.min = wf->ReadTupleLength( entity, "volume", 4, volume.z.min ); + volume.z.max = wf->ReadTupleLength( entity, "volume", 5, volume.z.max ); + + // force the windings for GL's sake + if( volume.x.min > volume.x.max ) + swap( volume.x.min, volume.x.max ); + + if( volume.y.min > volume.y.max ) + swap( volume.y.min, volume.y.max ); + + if( volume.z.min > volume.z.max ) + swap( volume.z.min, volume.z.max ); + + } + + watts = wf->ReadFloat( entity, "watts", watts ); +} Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/model.cc 2009-01-18 01:56:16 UTC (rev 7280) @@ -659,22 +659,29 @@ // this->world->sim_time, this->token, this->subs ); // f we're drawing current and a power pack has been installed - if( power_pack && (watts > 0) ) + if( power_pack ) { - // consume energy stored in the power pack - stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - power_pack->stored -= consumed; - - /* - printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", - token, - watts, - consumed, - power_pack, - power_pack->stored, - power_pack->capacity, - power_pack->stored / power_pack->capacity * 100.0 ); - */ + if( watts > 0 ) + { + // consume energy stored in the power pack + stg_joules_t consumed = watts * (world->interval_sim * 1e-6); + power_pack->stored -= consumed; + + /* + printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", + token, + watts, + consumed, + power_pack, + power_pack->stored, + power_pack->capacity, + power_pack->stored / power_pack->capacity * 100.0 ); + */ + } + + // I own this power pack, see if the world wants to recharge it */ + if( power_pack->mod == this ) + world->TryCharge( power_pack, GetGlobalPose() ); } CallCallbacks( &hooks.update ); Modified: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/model.hh 2009-01-18 01:56:16 UTC (rev 7280) @@ -23,6 +23,10 @@ /** OpenGL visualization of the powerpack state */ void Visualize( Camera* cam ); + + /** Print human-readable status on stdout, prefixed with the + argument string */ + void Print( char* prefix ); }; class Visibility Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/powerpack.cc 2009-01-18 01:56:16 UTC (rev 7280) @@ -2,23 +2,28 @@ Simple model of energy storage Richard Vaughan Created 2009.1.15 - $Id$ + SVN: $Id: stage.hh 7279 2009-01-18 00:10:21Z rtv $ */ #include "stage_internal.hh" PowerPack::PowerPack( Model* mod ) : - mod( mod), stored( 0.0 ), capacity( 0.0 ) + mod( mod), stored( 0.0 ), capacity( 0.0 ), charging( false ) { // nothing to do }; +void PowerPack::Print( char* prefix ) +{ + printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity ); +} + /** OpenGL visualization of the powerpack state */ void PowerPack::Visualize( Camera* cam ) { const double height = 0.5; - const double width = 0.3; + const double width = 0.2; double percent = stored/capacity * 100.0; @@ -42,13 +47,25 @@ // outline the charge-o-meter glTranslatef( 0,0,0.001 ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glColor4f( 0,0,0,0.7 ); + glRectf( 0,0,width, height ); - + glBegin( GL_LINES ); glVertex2f( 0, fullness ); glVertex2f( width, fullness ); glEnd(); + + if( charging ) + { + glLineWidth( 6.0 ); + glColor4f( 1,0,0,0.7 ); + + glRectf( 0,0,width, height ); + + glLineWidth( 1.0 ); + } // draw the percentage //gl_draw_string( -0.2, 0, 0, buf ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/stage.hh 2009-01-18 01:56:16 UTC (rev 7280) @@ -809,7 +809,24 @@ class Region; class SuperRegion; class BlockGroup; + class PowerPack; + /// %Charger class + class Charger + { + World* world; + stg_watts_t watts; + stg_bounds3d_t volume; + + public: + Charger( World* world ); + void ChargeIfContained( PowerPack* pp, Pose pose ); + bool Contains( Pose pose ); + void Charge( PowerPack* pp ); + void Visualize(); + void Load( Worldfile* wf, int entity ); + }; + /// %World class class World : public Ancestor { @@ -817,7 +834,8 @@ friend class Block; //friend class StgTime; friend class Canvas; - + friend class Charger; + private: static GList* world_list; ///< all the worlds that exist @@ -825,6 +843,7 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids + GList* chargers; bool destroy; bool dirty; ///< iff true, a gui redraw would be required GHashTable* models_by_name; ///< the models that make up the world, indexed by name @@ -866,6 +885,7 @@ void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); + void LoadCharger( Worldfile* wf, int entity ); SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); @@ -982,6 +1002,8 @@ void CancelQuit(){ quit = false; } void CancelQuitAll(){ quit_all = false; } + void TryCharge( PowerPack* pp, Pose pose ); + /** Get the resolution in pixels-per-metre of the underlying discrete raytracing model */ double Resolution(){ return ppm; }; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/libstage/world.cc 2009-01-18 01:56:16 UTC (rev 7280) @@ -69,6 +69,7 @@ double ppm ) : // private + chargers( NULL ), destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), @@ -310,20 +311,22 @@ for( int entity = 1; entity < wf->GetEntityCount(); entity++ ) { const char *typestr = (char*)wf->GetEntityType(entity); - + // don't load window entries here if( strcmp( typestr, "window" ) == 0 ) - { - /* do nothing here */ - } + { + /* do nothing here */ + } //else if( strcmp( typestr, "blockgroup" ) == 0 ) //LoadBlockGroup( wf, entity, entitytable ); else if( strcmp( typestr, "block" ) == 0 ) - LoadBlock( wf, entity, entitytable ); - else - LoadModel( wf, entity, entitytable ); + LoadBlock( wf, entity, entitytable ); + else if( strcmp( typestr, "charger" ) == 0 ) + LoadCharger( wf, entity ); + else + LoadModel( wf, entity, entitytable ); } - + // warn about unused WF lines wf->WarnUnused(); @@ -342,7 +345,16 @@ putchar( '\n' ); } +void World::LoadCharger( Worldfile* wf, int entity ) +{ + Charger* chg = new Charger( this ); + + chargers = g_list_prepend( chargers, chg ); + chg->Load( wf, entity ); +} + + // delete a model from the hash table static void destroy_model( gpointer dummy1, Model* mod, gpointer dummy2 ) { @@ -916,5 +928,16 @@ extent.z.max = MAX( extent.z.max, pt.z ); } +void World::TryCharge( PowerPack* pack, Pose pose ) +{ + pack->charging = false; + // see if the pose lies within any of the charging rectangles + for( GList* it = chargers; it; it = it->next ) + { + Charger* chg = (Charger*)it->data; + chg->ChargeIfContained( pack, pose ); + } +} + Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-18 00:10:21 UTC (rev 7279) +++ code/stage/trunk/worlds/fasr.world 2009-01-18 01:56:16 UTC (rev 7280) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 00 # real-time interval between simulation updates in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 @@ -20,9 +20,9 @@ # configure the GUI window window ( - size [ 600.000 600.000 ] + size [ 600.000 599.000 ] - center [ -0.521 -0.218 ] + center [ -0.424 -0.218 ] rotate [ 0 0 ] scale 30.883 @@ -45,6 +45,18 @@ bitmap "bitmaps/cave.png" ) +charger +( + volume [ 5 6 -2 -3 0 0.1 ] + watts 200 +) + +charger +( + volume [ 5 6 -4 -5 0 0.1 ] + watts 200 +) + zone ( color "green" @@ -61,12 +73,6 @@ ctrl "sink" ) -#charger -#( -# pose -# -#) - define autorob pioneer2dx ( sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) @@ -78,9 +84,9 @@ autorob( pose [6.471 5.304 0 14.941] ) autorob( pose [5.937 4.858 0 -147.503] ) autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.664 5.938 0 -51.799] ) +autorob( pose [5.664 5.938 0 107.666] ) autorob( pose [7.016 6.428 0 -128.279] ) -autorob( pose [5.911 4.040 0 -97.047] ) +autorob( pose [5.750 4.137 0 -97.047] ) autorob( pose [4.909 6.097 0 -44.366] ) autorob( pose [6.898 4.775 0 -117.576] ) autorob( pose [7.012 5.706 0 129.497] ) @@ -88,13 +94,13 @@ autorob( pose [6.616 6.893 0 170.743] ) autorob( pose [6.451 4.189 0 -61.453] ) autorob( pose [5.098 6.788 0 -61.295] ) -autorob( pose [4.374 5.163 0 -90.417] ) -autorob( pose [4.999 4.230 0 -42.157] ) -autorob( pose [4.331 4.217 0 -95.000] ) +autorob( pose [4.374 5.163 0 -147.713] ) +autorob( pose [4.999 4.230 0 -125.236] ) +autorob( pose [4.007 4.249 0 78.789] ) autorob( pose [5.440 5.317 0 -26.545] ) autorob( pose [7.518 6.973 0 163.239] ) autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [4.839 3.595 0 -179.567] ) +autorob( pose [5.940 6.768 0 77.301] ) #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [6.203 6.963 0 2.937] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-23 22:38:01
|
Revision: 7290 http://playerstage.svn.sourceforge.net/playerstage/?rev=7290&view=rev Author: rtv Date: 2009-01-23 21:49:08 +0000 (Fri, 23 Jan 2009) Log Message: ----------- bugfixes Modified Paths: -------------- code/stage/trunk/INSTALL.txt code/stage/trunk/RELEASE.txt code/stage/trunk/docsrc/Makefile code/stage/trunk/docsrc/header.html code/stage/trunk/docsrc/stage.dox code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/model.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/INSTALL.txt =================================================================== --- code/stage/trunk/INSTALL.txt 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/INSTALL.txt 2009-01-23 21:49:08 UTC (rev 7290) @@ -1,10 +1,11 @@ Build system ------------ -Stage is now built using the CMake build system (version 2.4.7 or newer). -This has two main advantages: (i) it is much faster; (ii) CMake can -create native build files for Windows and Mac OS X, which will help -Stage become more portable. +Stage is now built using the CMake build system (version 2.4.7 or +newer). This has two main advantages over the old GNU +autoconf/automake system: (i) it is much faster; (ii) CMake can create +native build files for Windows and Mac OS X, which will help Stage +become more portable. Dependencies ------------ Modified: code/stage/trunk/RELEASE.txt =================================================================== --- code/stage/trunk/RELEASE.txt 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/RELEASE.txt 2009-01-23 21:49:08 UTC (rev 7290) @@ -1,3 +1,10 @@ +Version 3.1.0 +------------- + +XX TODO + +Richard Vaughan (rtv) va...@sf... - 2009.X.X + Version 3.0.1 ------------- This version incorporates a number of fixes made since the previous Modified: code/stage/trunk/docsrc/Makefile =================================================================== --- code/stage/trunk/docsrc/Makefile 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/docsrc/Makefile 2009-01-23 21:49:08 UTC (rev 7290) @@ -4,7 +4,6 @@ EXTRA_DIST = \ README \ stage.txt \ -sourced.txt \ header.html \ stage.dox \ stage_button.png Modified: code/stage/trunk/docsrc/header.html =================================================================== --- code/stage/trunk/docsrc/header.html 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/docsrc/header.html 2009-01-23 21:49:08 UTC (rev 7290) @@ -147,7 +147,7 @@ <div class=box> <div class="title">Developer</div> -<a href="group__libstage.html">libstage</a> +<a href="namespaces.html">libstage</a> </div> <div class=box_light> Modified: code/stage/trunk/docsrc/stage.dox =================================================================== --- code/stage/trunk/docsrc/stage.dox 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/docsrc/stage.dox 2009-01-23 21:49:08 UTC (rev 7290) @@ -23,7 +23,7 @@ # This could be handy for archiving the generated documentation or # if some version control system is used. -PROJECT_NUMBER = 3.0.1 +PROJECT_NUMBER = 3.1.0 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) # base path where the generated documentation will be put. Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-23 21:49:08 UTC (rev 7290) @@ -33,6 +33,7 @@ ModelLaser* laser; ModelRanger* ranger; ModelBlobfinder* blobfinder; + ModelFiducial* fiducial; Model *source, *sink; int avoidcount, randcount; int work_get, work_put; @@ -52,15 +53,18 @@ robot->pos = (ModelPosition*)mod; - robot->laser = (ModelLaser*)mod->GetModel( "laser:0" ); + robot->laser = (ModelLaser*)mod->GetUnusedModelOfType( MODEL_TYPE_LASER ); assert( robot->laser ); robot->laser->Subscribe(); - robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" ); + robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL ); + assert( robot->fiducial ); + robot->fiducial->Subscribe(); + + robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( MODEL_TYPE_RANGER ); assert( robot->ranger ); //robot->ranger->Subscribe(); - robot->avoidcount = 0; robot->randcount = 0; @@ -73,21 +77,6 @@ robot->sink = mod->GetWorld()->GetModel( "sink" ); assert(robot->sink); - -// const int waypoint_count = 100; -// Waypoint* waypoints = new Waypoint[waypoint_count]; - -// for( int i=0; i<waypoint_count; i++ ) -// { -// waypoints[i].pose.x = i* 0.1; -// waypoints[i].pose.y = drand48() * 4.0; -// waypoints[i].pose.z = 0; -// waypoints[i].pose.a = normalize( i/10.0 ); -// waypoints[i].color = stg_color_pack( 0,0,1,0 ); -// } - -// robot->pos->SetWaypoints( waypoints, waypoint_count ); - return 0; //ok } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/libstage/model.cc 2009-01-23 21:49:08 UTC (rev 7290) @@ -489,39 +489,36 @@ // returns true iff model [testmod] is a descendent of this model bool Model::IsDescendent( Model* testmod ) { + if( this == testmod ) + return true; + for( GList* it=this->children; it; it=it->next ) { - Model* child = (Model*)it->data; - - if( child == testmod ) - return true; - + Model* child = (Model*)it->data; if( child->IsDescendent( testmod ) ) - return true; + return true; } - + // neither mod nor a child of this matches testmod return false; } -// returns true iff model [mod1] and [mod2] are in the same model tree -bool Model::IsRelated( Model* mod2 ) +bool Model::IsRelated( Model* that ) { - return( (this == mod2) || IsAntecedent( mod2 ) || IsDescendent( mod2 ) ); + // is it me? + if( this == that ) + return true; + + // wind up to top-level object + Model* candidate = this; + while( candidate->parent ) + candidate = candidate->parent; + + // and recurse down the tree + return candidate->IsDescendent( that ); } -// bool Model::IsRelated( Model* that ) -// { -// if( this == that ) -// return true; - -// for( GList* it = children; it; it=it->next ) -// { -// if( -// } - - // get the model's velocity in the global frame Velocity Model::GetGlobalVelocity() { Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-23 19:44:12 UTC (rev 7289) +++ code/stage/trunk/worlds/fasr.world 2009-01-23 21:49:08 UTC (rev 7290) @@ -75,9 +75,11 @@ define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + fiducial( ) ctrl "fasr" joules 1000000 + fiducial_return 1 ) autorob( pose [4.116 6.107 0 -147.323] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-26 23:07:34
|
Revision: 7304 http://playerstage.svn.sourceforge.net/playerstage/?rev=7304&view=rev Author: rtv Date: 2009-01-26 22:42:12 +0000 (Mon, 26 Jan 2009) Log Message: ----------- fixed libstageplugin. some usuability issues remain, but it works Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/docsrc/stage.dox code/stage/trunk/libstage/canvas.hh code/stage/trunk/libstageplugin/CMakeLists.txt code/stage/trunk/libstageplugin/p_blobfinder.cc code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_driver.h code/stage/trunk/libstageplugin/p_fiducial.cc code/stage/trunk/libstageplugin/p_laser.cc code/stage/trunk/libstageplugin/p_position.cc code/stage/trunk/libstageplugin/p_simulation.cc code/stage/trunk/libstageplugin/p_sonar.cc code/stage/trunk/libstageplugin/p_speech.cc code/stage/trunk/libstageplugin/stg_time.cc code/stage/trunk/worlds/simple.cfg code/stage/trunk/worlds/simple.world Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/CMakeLists.txt 2009-01-26 22:42:12 UTC (rev 7304) @@ -141,7 +141,7 @@ 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_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 ) Modified: code/stage/trunk/docsrc/stage.dox =================================================================== --- code/stage/trunk/docsrc/stage.dox 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/docsrc/stage.dox 2009-01-26 22:42:12 UTC (rev 7304) @@ -87,7 +87,7 @@ # "The $name file" "is" "provides" "specifies" "contains" # "represents" "a" "an" "the" -ABBREVIATE_BRIEF = NO +ABBREVIATE_BRIEF = YES # If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then # Doxygen will generate a detailed section even if there is only a brief @@ -219,7 +219,7 @@ # Private class members and static file members will be hidden unless # the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES -EXTRACT_ALL = NO +EXTRACT_ALL = YES # If the EXTRACT_PRIVATE tag is set to YES all private members of a class # will be included in the documentation. @@ -1138,7 +1138,7 @@ # So in most cases it will be better to enable call graphs for selected # functions only using the \callgraph command. -CALL_GRAPH = YES +CALL_GRAPH = NO # If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen # will graphical hierarchy of all classes instead of a textual one. Modified: code/stage/trunk/libstage/canvas.hh =================================================================== --- code/stage/trunk/libstage/canvas.hh 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstage/canvas.hh 2009-01-26 22:42:12 UTC (rev 7304) @@ -84,7 +84,7 @@ visualizeAll; public: - Canvas( WorldGui* world, int x, int y, int width, int height); + Canvas( WorldGui* world, int x, int y, int width, int height); ~Canvas(); bool graphics; Modified: code/stage/trunk/libstageplugin/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/CMakeLists.txt 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/CMakeLists.txt 2009-01-26 22:42:12 UTC (rev 7304) @@ -15,9 +15,11 @@ stg_time.cc ) +# TODO +# p_graphics3d.cc + add_library( stageplugin MODULE ${stagepluginSrcs} ) -# p_graphics3d.cc target_link_libraries( stageplugin @@ -30,6 +32,9 @@ set_source_files_properties( ${stagepluginSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) +# delete the "lib" prefix from the plugin +SET_TARGET_PROPERTIES( stageplugin PROPERTIES PREFIX "" ) + IF (BUILD_LSPTEST) ADD_SUBDIRECTORY(test) ENDIF (BUILD_LSPTEST) Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -36,6 +36,7 @@ // CODE #include "p_driver.h" +using namespace Stg; InterfaceBlobfinder::InterfaceBlobfinder( player_devaddr_t addr, StgDriver* driver, Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_driver.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -150,6 +150,7 @@ #include <math.h> #include "p_driver.h" +using namespace Stg; const char* copyright_notice = "\n * Part of the Player Project [http://playerstage.sourceforge.net]\n" Modified: code/stage/trunk/libstageplugin/p_driver.h =================================================================== --- code/stage/trunk/libstageplugin/p_driver.h 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_driver.h 2009-01-26 22:42:12 UTC (rev 7304) @@ -7,9 +7,8 @@ #include <libplayercore/playercore.h> -#include "../libstage/stage_internal.hh" +#include "../libstage/stage.hh" - #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X ) // foward declare; @@ -39,15 +38,15 @@ virtual void Update(); /// all player devices share the same Stage world (for now) - static WorldGui* world; + static Stg::WorldGui* world; /// find the device record with this Player id Interface* LookupDevice( player_devaddr_t addr ); - Model* LocateModel( char* basename, - player_devaddr_t* addr, - stg_model_type_t type ); - + Stg::Model* LocateModel( char* basename, + player_devaddr_t* addr, + Stg::stg_model_type_t type ); + protected: /// an array of pointers to Interface objects, defined below @@ -101,9 +100,9 @@ StgDriver* driver, ConfigFile* cf, int section, - stg_model_type_t type ); + Stg::stg_model_type_t type ); - Model* mod; + Stg::Model* mod; virtual ~InterfaceModel( void ){ /* TODO: clean up*/ }; Modified: code/stage/trunk/libstageplugin/p_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -45,6 +45,7 @@ // CODE #include "p_driver.h" +using namespace Stg; InterfaceFiducial::InterfaceFiducial( player_devaddr_t addr, Modified: code/stage/trunk/libstageplugin/p_laser.cc =================================================================== --- code/stage/trunk/libstageplugin/p_laser.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_laser.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -39,6 +39,7 @@ // CODE ---------------------------------------------------------------------- #include "p_driver.h" +using namespace Stg; InterfaceLaser::InterfaceLaser( player_devaddr_t addr, StgDriver* driver, Modified: code/stage/trunk/libstageplugin/p_position.cc =================================================================== --- code/stage/trunk/libstageplugin/p_position.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_position.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -43,9 +43,8 @@ // CODE ---------------------------------------------------------------------- #include "p_driver.h" -//#include "playerclient.h" +using namespace Stg; - InterfacePosition::InterfacePosition( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, Modified: code/stage/trunk/libstageplugin/p_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/p_simulation.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_simulation.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -52,6 +52,7 @@ #include <libplayercore/globals.h> // for player_argc & player_argv #include "p_driver.h" +using namespace Stg; // these are Player globals extern bool player_quiet_startup; Modified: code/stage/trunk/libstageplugin/p_sonar.cc =================================================================== --- code/stage/trunk/libstageplugin/p_sonar.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_sonar.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -37,6 +37,7 @@ // CODE ---------------------------------------------------------------------- #include "p_driver.h" +using namespace Stg; // // SONAR INTERFACE Modified: code/stage/trunk/libstageplugin/p_speech.cc =================================================================== --- code/stage/trunk/libstageplugin/p_speech.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/p_speech.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -28,6 +28,7 @@ #include "p_driver.h" +using namespace Stg; /** @addtogroup player @par Speech interface Modified: code/stage/trunk/libstageplugin/stg_time.cc =================================================================== --- code/stage/trunk/libstageplugin/stg_time.cc 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/libstageplugin/stg_time.cc 2009-01-26 22:42:12 UTC (rev 7304) @@ -26,8 +26,10 @@ // /////////////////////////////////////////////////////////////////////////// +#include <math.h> + #include "p_driver.h" -#include "math.h" +using namespace Stg; // Constructor StTime::StTime( StgDriver* driver ) Modified: code/stage/trunk/worlds/simple.cfg =================================================================== --- code/stage/trunk/worlds/simple.cfg 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/worlds/simple.cfg 2009-01-26 22:42:12 UTC (rev 7304) @@ -11,7 +11,7 @@ ( name "stage" provides [ "simulation:0" ] - plugin "libstageplugin" + plugin "stageplugin" # load the named file into the simulator worldfile "simple.world" Modified: code/stage/trunk/worlds/simple.world =================================================================== --- code/stage/trunk/worlds/simple.world 2009-01-26 22:41:12 UTC (rev 7303) +++ code/stage/trunk/worlds/simple.world 2009-01-26 22:42:12 UTC (rev 7304) @@ -7,9 +7,9 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 10 # real-time interval between simulation updates in milliseconds +interval_real 100 # real-time interval between simulation updates in milliseconds -paused 1 +paused 0 resolution 0.02 @@ -17,7 +17,8 @@ window ( size [ 556.000 557.000 ] # in pixels - scale 28.116 # pixels per meter + scale 28.116 + # pixels per meter center [ 8.058 7.757 ] rotate [ 0 0 ] @@ -42,6 +43,6 @@ pose [ 0.892 0.800 0 56.500 ] sicklaser() - ctrl "wander" + #ctrl "wander" ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-28 09:05:31
|
Revision: 7308 http://playerstage.svn.sourceforge.net/playerstage/?rev=7308&view=rev Author: rtv Date: 2009-01-28 09:05:20 +0000 (Wed, 28 Jan 2009) Log Message: ----------- improved fasr example. working on reparenting ready for gripper - not yet functional Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/charger.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -1,17 +1,18 @@ #include "stage.hh" using namespace Stg; +const bool verbose = false; + +// navigation control params const double cruisespeed = 0.4; const double avoidspeed = 0.05; const double avoidturn = 0.5; const double minfrontdistance = 0.7; const double stopdist = 0.5; -const bool verbose = false; const int avoidduration = 10; const int workduration = 20; const int payload = 4; - double have[4][4] = { { 90, 180, 180, 180 }, { 90, -90, 0, -90 }, @@ -25,10 +26,11 @@ { -90, -90, 180, 180 }, { -90, -180, -90, -90 } }; - -typedef struct + +class Robot { +private: ModelPosition* pos; ModelLaser* laser; ModelRanger* ranger; @@ -37,56 +39,72 @@ Model *source, *sink; int avoidcount, randcount; int work_get, work_put; + + static int LaserUpdate( ModelLaser* mod, Robot* robot ); + static int PositionUpdate( ModelPosition* mod, Robot* robot ); + static int FiducialUpdate( ModelFiducial* mod, Robot* robot ); -} robot_t; +public: + Robot( ModelPosition* pos, + ModelLaser* laser, + ModelRanger* ranger, + ModelFiducial* fiducial, + ModelBlobfinder* blob, + Model* source, + Model* sink ) + : pos(pos), + laser(laser), + ranger(ranger), + blobfinder(blobfinder), + fiducial(fiducial), + source(source), + sink(sink), + avoidcount(0), + randcount(0), + work_get(0), + work_put(0) + { + // need at least these models to get any work done + assert( pos ); + assert( laser ); + + pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); + laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this ); -int LaserUpdate( Model* mod, robot_t* robot ); -int PositionUpdate( Model* mod, robot_t* robot ); + if( fiducial ) // optional + fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); + } +}; + // Stage calls this when the model starts up extern "C" int Init( Model* mod ) { - robot_t* robot = new robot_t; - robot->work_get = 0; - robot->work_put = 0; - - robot->pos = (ModelPosition*)mod; - - robot->laser = (ModelLaser*)mod->GetUnusedModelOfType( MODEL_TYPE_LASER ); - assert( robot->laser ); - robot->laser->Subscribe(); - - robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL ); - assert( robot->fiducial ); - robot->fiducial->Subscribe(); - - robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( MODEL_TYPE_RANGER ); - assert( robot->ranger ); - //robot->ranger->Subscribe(); - - robot->avoidcount = 0; - robot->randcount = 0; - - robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot ); - robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot ); - - robot->source = mod->GetWorld()->GetModel( "source" ); - assert(robot->source); - - robot->sink = mod->GetWorld()->GetModel( "sink" ); - assert(robot->sink); + Robot* robot = new Robot( (ModelPosition*)mod, + (ModelLaser*)mod->GetUnusedModelOfType( MODEL_TYPE_LASER ), + (ModelRanger*)mod->GetUnusedModelOfType( MODEL_TYPE_RANGER ), + (ModelFiducial*)mod->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL ), + NULL, + mod->GetWorld()->GetModel( "source" ), + mod->GetWorld()->GetModel( "sink" ) ); return 0; //ok } // inspect the laser data and decide what to do -int LaserUpdate( Model* mod, robot_t* robot ) +int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) { - // get the data + if( laser->power_pack && laser->power_pack->charging ) + printf( "model %s power pack @%p is charging\n", + laser->Token(), laser->power_pack ); + + // Get the data uint32_t sample_count=0; - stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count ); - assert(scan); + stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); + + if( scan == NULL ) + return 0; bool obstruction = false; bool stop = false; @@ -194,9 +212,9 @@ return 0; } -int PositionUpdate( Model* mod, robot_t* robot ) +int Robot::PositionUpdate( ModelPosition* pos, Robot* robot ) { - Pose pose = robot->pos->GetPose(); + Pose pose = pos->GetPose(); //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", // pose.x, pose.y, pose.z, pose.a ); @@ -204,7 +222,7 @@ //pose.z += 0.0001; //robot->pos->SetPose( pose ); - if( robot->pos->GetFlagCount() < payload && + if( pos->GetFlagCount() < payload && hypot( -7-pose.x, -7-pose.y ) < 2.0 ) { if( ++robot->work_get > workduration ) @@ -213,7 +231,7 @@ robot->source->Lock(); // transfer a chunk from source to robot - robot->pos->PushFlag( robot->source->PopFlag() ); + pos->PushFlag( robot->source->PopFlag() ); robot->source->Unlock(); robot->work_get = 0; @@ -229,7 +247,7 @@ //puts( "dropping" ); // transfer a chunk between robot and goal - robot->sink->PushFlag( robot->pos->PopFlag() ); + robot->sink->PushFlag( pos->PopFlag() ); robot->sink->Unlock(); robot->work_put = 0; @@ -240,3 +258,27 @@ return 0; // run again } + + +int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot ) +{ + for( unsigned int i = 0; i < mod->fiducial_count; i++ ) + { + stg_fiducial_t* f = &mod->fiducials[i]; + + //printf( "fiducial %d is %d at %.2f m %.2f radians\n", + // i, f->id, f->range, f->bearing ); + + if( f->range < 1 ) + { + printf( "attempt to grab model @%p %s\n", + f->mod, f->mod->Token() ); + + // working on picking up models + //robot->pos->BecomeParentOf( f->mod ); + } + + } + + return 0; // run again +} Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-28 09:05:20 UTC (rev 7308) @@ -28,6 +28,7 @@ options_dlg.cc options_dlg.hh powerpack.cc + puck.cc region.cc resource.cc stage.cc Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/ancestor.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -1,15 +1,14 @@ #include "stage.hh" using namespace Stg; -Ancestor::Ancestor() +Ancestor::Ancestor() : + children( NULL ), + debug( false ), + puck_list( NULL ), + token( NULL ) { - token = NULL; - children = NULL; - - for( int i=0; i<MODEL_TYPE_COUNT; i++ ) - child_type_counts[i] = 0; - - debug = false; + for( int i=0; i<MODEL_TYPE_COUNT; i++ ) + child_type_counts[i] = 0; } Ancestor::~Ancestor() @@ -20,8 +19,7 @@ delete (Model*)it->data; g_list_free( children ); - } - + } } void Ancestor::AddChild( Model* mod ) @@ -73,3 +71,12 @@ } } + + +void Ancestor::Load( Worldfile* wf, int section ) +{ +} + +void Ancestor::Save( Worldfile* wf, int section ) +{ +} Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/canvas.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -804,6 +804,9 @@ if( showBBoxes ) DrawBoundingBoxes(); + + LISTMETHOD( world->puck_list, Puck*, Draw ); + // TODO - finish this properly //LISTMETHOD( models_sorted, Model*, DrawWaypoints ); Modified: code/stage/trunk/libstage/charger.cc =================================================================== --- code/stage/trunk/libstage/charger.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/charger.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -13,7 +13,7 @@ Charger::Charger( World* world ) : world( world ), watts( 1000.0 ) { - printf( "Charger constructed" ); + //printf( "Charger constructed" ); } void Charger::ChargeIfContained( PowerPack* pp, Pose pose ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/model.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -126,6 +126,37 @@ uint32_t Model::count = 0; GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); + +void Size::Load( Worldfile* wf, int section, const char* keyword ) +{ + x = wf->ReadTupleLength( section, keyword, 0, x ); + y = wf->ReadTupleLength( section, keyword, 1, y ); + z = wf->ReadTupleLength( section, keyword, 2, z ); +} + +void Size::Save( Worldfile* wf, int section, const char* keyword ) +{ + wf->WriteTupleLength( section, keyword, 0, x ); + wf->WriteTupleLength( section, keyword, 1, y ); + wf->WriteTupleLength( section, keyword, 2, z ); +} + +void Pose::Load( Worldfile* wf, int section, const char* keyword ) +{ + x = wf->ReadTupleLength( section, keyword, 0, x ); + y = wf->ReadTupleLength( section, keyword, 1, y ); + z = wf->ReadTupleLength( section, keyword, 2, z ); + a = wf->ReadTupleAngle( section, keyword, 3, a ); +} + +void Pose::Save( Worldfile* wf, int section, const char* keyword ) +{ + wf->WriteTupleLength( section, keyword, 0, x ); + wf->WriteTupleLength( section, keyword, 1, y ); + wf->WriteTupleLength( section, keyword, 2, z ); + wf->WriteTupleAngle( section, keyword, 3, a ); +} + Visibility::Visibility() : blob_return( true ), fiducial_key( 0 ), @@ -166,7 +197,7 @@ // constructor Model::Model( World* world, Model* parent, - const stg_model_type_t type ) + const stg_model_type_t type ) : Ancestor(), access_mutex(NULL), blinkenlights( g_ptr_array_new() ), @@ -177,7 +208,7 @@ color( 0xFFFF0000 ), // red data_fresh(false), disabled(false), - custom_visual_list( NULL ), + custom_visual_list( NULL ), flag_list(NULL), geom(), has_default_block( true ), @@ -705,7 +736,7 @@ { return( world->sim_time >= (last_update + interval) ); } - + void Model::Update( void ) { // printf( "[%llu] %s update (%d subs)\n", @@ -1748,3 +1779,15 @@ { blockgroup.UnMap(); } + +void Model::BecomeParentOf( Model* child ) +{ + if( child->parent ) + child->parent->RemoveChild( child ); + + child->parent = this; + + this->AddChild( child ); + + world->dirty = true; +} Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -73,8 +73,12 @@ { //puts( "callback was not installed" ); } + + // return the number of callbacks now in the list. Useful for + // detecting when the list is empty. + //return g_list_length( cb_list ); - return 0; //ok + return 0; } Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -177,46 +177,44 @@ NULL, false ); - range = ray.range; + //range = ray.range; Model* hitmod = ray.mod; // printf( "ray hit %s and was seeking LOS to %s\n", // hitmod ? hitmod->Token() : "null", // him->Token() ); - //assert( ! (hitmod == this) ); - // if it was him, we can see him if( hitmod == him ) - { - Geom hisgeom = him->GetGeom(); + { + Geom hisgeom = him->GetGeom(); + + // record where we saw him and what he looked like + stg_fiducial_t fid; + fid.mod = him; + fid.range = range; + fid.bearing = dtheta; + fid.geom.x = hisgeom.size.x; + fid.geom.y = hisgeom.size.y; + 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)); - // record where we saw him and what he looked like - stg_fiducial_t fid; - fid.range = range; - fid.bearing = dtheta; - fid.geom.x = hisgeom.size.x; - fid.geom.y = hisgeom.size.y; - 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)); - - // if he's within ID range, get his fiducial.return value, else - // we see value 0 - fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0; - - PRINT_DEBUG2( "adding %s's value %d to my list of fiducials", - him->Token(), him->vis.fiducial_return ); - - g_array_append_val( data, fid ); - } - + // if he's within ID range, get his fiducial.return value, else + // we see value 0 + fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0; + + 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; } - /////////////////////////////////////////////////////////////////////////// // Update the beacon data // @@ -265,22 +263,22 @@ return; // draw the FOV - // GLUquadric* quadric = gluNewQuadric(); + GLUquadric* quadric = gluNewQuadric(); - // PushColor( 0,0,0,0.2 ); + PushColor( 0,0,0,0.2 ); - // gluQuadricDrawStyle( quadric, GLU_SILHOUETTE ); + 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 + 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 ); - // PopColor(); + gluDeleteQuadric( quadric ); + PopColor(); if( data->len == 0 ) return; Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/model_load.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -99,49 +99,38 @@ if( wf->PropertyExists( wf_entity, "origin" ) ) { Geom geom = GetGeom(); - geom.pose.x = wf->ReadTupleLength(wf_entity, "origin", 0, geom.pose.x ); - geom.pose.y = wf->ReadTupleLength(wf_entity, "origin", 1, geom.pose.y ); - geom.pose.z = wf->ReadTupleLength(wf_entity, "origin", 2, geom.pose.z ); - geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin", 3, geom.pose.a ); - this->SetGeom( geom ); + geom.pose.Load( wf, wf_entity, "origin" ); + SetGeom( geom ); } - + if( wf->PropertyExists( wf_entity, "size" ) ) { Geom geom = GetGeom(); - geom.size.x = wf->ReadTupleLength(wf_entity, "size", 0, geom.size.x ); - geom.size.y = wf->ReadTupleLength(wf_entity, "size", 1, geom.size.y ); - geom.size.z = wf->ReadTupleLength(wf_entity, "size", 2, geom.size.z ); - this->SetGeom( geom ); + geom.size.Load( wf, wf_entity, "size" ); + SetGeom( geom ); } - + if( wf->PropertyExists( wf_entity, "pose" )) { Pose pose = GetPose(); - pose.x = wf->ReadTupleLength(wf_entity, "pose", 0, pose.x ); - pose.y = wf->ReadTupleLength(wf_entity, "pose", 1, pose.y ); - pose.z = wf->ReadTupleLength(wf_entity, "pose", 2, pose.z ); - pose.a = wf->ReadTupleAngle(wf_entity, "pose", 3, pose.a ); - this->SetPose( pose ); + pose.Load( wf, wf_entity, "pose" ); + SetPose( pose ); } - + if( wf->PropertyExists( wf_entity, "velocity" )) { Velocity vel = GetVelocity(); - vel.x = wf->ReadTupleLength(wf_entity, "velocity", 0, vel.x ); - vel.y = wf->ReadTupleLength(wf_entity, "velocity", 1, vel.y ); - vel.z = wf->ReadTupleLength(wf_entity, "velocity", 2, vel.z ); - vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a ); - this->SetVelocity( vel ); + vel.Load( wf, wf_entity, "velocity" ); + SetVelocity( vel ); } - + if( wf->PropertyExists( wf_entity, "color" )) { stg_color_t col = 0xFFFF0000; // red; const char* colorstr = wf->ReadString( wf_entity, "color", NULL ); if( colorstr ) - { - if( strcmp( colorstr, "random" ) == 0 ) + { + if( strcmp( colorstr, "random" ) == 0 ) { col = (uint32_t)random(); col |= 0xFF000000; // set the alpha channel to max Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/stage.hh 2009-01-28 09:05:20 UTC (rev 7308) @@ -220,7 +220,7 @@ { public: stg_meters_t x, y, z; - + Size( stg_meters_t x, stg_meters_t y, stg_meters_t z ) @@ -230,6 +230,9 @@ /** default constructor uses default non-zero values */ Size() : x( 0.1 ), y( 0.1 ), z( 0.1 ) {/*empty*/} + + void Load( Worldfile* wf, int section, const char* keyword ); + void Save( Worldfile* wf, int section, const char* keyword ); }; /** Specify a 3 axis position, in x, y and heading. */ @@ -270,6 +273,9 @@ printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x,y,z,a ); } + + void Load( Worldfile* wf, int section, const char* keyword ); + void Save( Worldfile* wf, int section, const char* keyword ); }; @@ -778,8 +784,26 @@ /** Define a callback function type that can be attached to a record within a model and called whenever the record is set.*/ typedef int (*stg_model_callback_t)( Model* mod, void* user ); + + class Puck + { + private: + void BuildDisplayList(); - + public: + stg_color_t color; + int displaylist; + stg_meters_t height; + Pose pose; + stg_meters_t radius; + + Puck(); + void Load( Worldfile* wf, int section ); + void Save( Worldfile* wf, int section ); + + void Draw(); + }; + // ANCESTOR CLASS /** Base class for Model and World */ class Ancestor @@ -788,9 +812,13 @@ protected: GList* children; + bool debug; + GList* puck_list; char* token; - bool debug; + void Load( Worldfile* wf, int section ); + void Save( Worldfile* wf, int section ); + public: /** recursively call func( model, arg ) for each descendant */ @@ -810,7 +838,7 @@ { return token; } void SetToken( const char* str ) - { token = strdup( str ); } // teeny memory leak + { token = strdup( str ); } // little memory leak }; /** raytrace sample @@ -906,6 +934,7 @@ void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); + void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadCharger( Worldfile* wf, int entity ); SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); @@ -1591,8 +1620,6 @@ global coordinate frame is the parent is NULL. */ Pose pose; - /** Optional attached PowerPack, defaults to NULL */ - PowerPack* power_pack; /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code @@ -1620,6 +1647,9 @@ Visibility vis; + /** Optional attached PowerPack, defaults to NULL */ + PowerPack* power_pack; + void Lock() { if( access_mutex == NULL ) @@ -1795,7 +1825,8 @@ /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); - + void BecomeParentOf( Model* child ); + void Load( Worldfile* wf, int wf_entity ) { /** Set the worldfile and worldfile entity ID - must be called @@ -2006,12 +2037,18 @@ void RemoveSaveCallback( stg_model_callback_t cb ) { RemoveCallback( &hooks.save, cb ); } - + void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.update, cb, user ); } + { + AddCallback( &hooks.update, cb, user ); + Subscribe(); // if attaching a callback here, assume we want updates to happen + } void RemoveUpdateCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.update, cb ); } + { + RemoveCallback( &hooks.update, cb ); + Unsubscribe(); + } /** named-property interface */ @@ -2331,8 +2368,8 @@ 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!) - int id; ///< the identifier of the target, or -1 if none can be detected. - + 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 Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/libstage/world.cc 2009-01-28 09:05:20 UTC (rev 7308) @@ -190,6 +190,19 @@ mod->LoadBlock( wf, entity ); } +void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ) +{ +// // lookup the group in which this was defined +// Ancestor* anc = (Ancestor*)g_hash_table_lookup( entitytable, +// (gpointer)wf->GetEntityParent( entity ) ); + + + Puck* puck = new Puck(); + puck->Load( wf, entity ); + puck_list = g_list_prepend( puck_list, puck ); +} + + void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ) { int parent_entity = wf->GetEntityParent( entity ); @@ -321,10 +334,10 @@ { /* do nothing here */ } - //else if( strcmp( typestr, "blockgroup" ) == 0 ) - //LoadBlockGroup( wf, entity, entitytable ); else if( strcmp( typestr, "block" ) == 0 ) LoadBlock( wf, entity, entitytable ); + else if( strcmp( typestr, "puck" ) == 0 ) + LoadPuck( wf, entity, entitytable ); else if( strcmp( typestr, "charger" ) == 0 ) LoadCharger( wf, entity ); else Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-27 08:59:09 UTC (rev 7307) +++ code/stage/trunk/worlds/fasr.world 2009-01-28 09:05:20 UTC (rev 7308) @@ -22,9 +22,9 @@ ( size [ 600.000 599.000 ] - center [ -0.424 -0.218 ] + center [ 0.051 -0.204 ] rotate [ 0 0 ] - scale 30.883 + scale 33.822 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -73,15 +73,20 @@ ctrl "sink" ) +#puck( pose [ 0 0 0 0 ] ) +#puck( pose [ 1 0 0 0 ] ) +#puck( pose [ 2 0 0 0 ] ) +#puck( pose [ 3 0 0 0 ] ) + define autorob pioneer2dx ( sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) - fiducial( ) ctrl "fasr" joules 1000000 fiducial_return 1 ) +#autorob( pose [4.116 6.107 0 -147.323] fiducial( range_max 3 ) ) autorob( pose [4.116 6.107 0 -147.323] ) autorob( pose [6.471 5.304 0 14.941] ) autorob( pose [5.937 4.858 0 -147.503] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-29 23:52:57
|
Revision: 7310 http://playerstage.svn.sourceforge.net/playerstage/?rev=7310&view=rev Author: rtv Date: 2009-01-29 23:52:51 +0000 (Thu, 29 Jan 2009) Log Message: ----------- added missing experimental file Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/canvas.hh code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world Added Paths: ----------- code/stage/trunk/libstage/puck.cc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-28 09:14:36 UTC (rev 7309) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-29 23:52:51 UTC (rev 7310) @@ -262,13 +262,16 @@ //printf( "fiducial %d is %d at %.2f m %.2f radians\n", // i, f->id, f->range, f->bearing ); + // if( 0 ) if( f->range < 1 ) { printf( "attempt to grab model @%p %s\n", f->mod, f->mod->Token() ); // working on picking up models - //robot->pos->BecomeParentOf( f->mod ); + robot->pos->BecomeParentOf( f->mod ); + f->mod->SetPose( Pose(0,0,0,0) ); + f->mod->Disable(); } } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-28 09:14:36 UTC (rev 7309) +++ code/stage/trunk/libstage/canvas.cc 2009-01-29 23:52:51 UTC (rev 7310) @@ -598,6 +598,12 @@ models_sorted = g_list_append( models_sorted, mod ); } +void Canvas::RemoveModel( Model* mod ) +{ + printf( "removing model %s from canvas list\n", mod->Token() ); + models_sorted = g_list_remove( models_sorted, mod ); +} + void Canvas::DrawGlobalGrid() { @@ -675,6 +681,14 @@ void Canvas::DrawBlocks() { LISTMETHOD( models_sorted, Model*, DrawBlocksTree ); + + // some models may be carried by others - this prevents them being drawn twice +// for( GList* it = models_sorted; it; it=it->next ) +// { +// Model* mod = (Model*)it->data; +// if( mod->parent == NULL ) +// mod->DrawBlocksTree(); +// } } void Canvas::DrawBoundingBoxes() Modified: code/stage/trunk/libstage/canvas.hh =================================================================== --- code/stage/trunk/libstage/canvas.hh 2009-01-28 09:14:36 UTC (rev 7309) +++ code/stage/trunk/libstage/canvas.hh 2009-01-29 23:52:51 UTC (rev 7310) @@ -62,6 +62,7 @@ void DrawGlobalGrid(); void AddModel( Model* mod ); + void RemoveModel( Model* mod ); Option showBlinken, showBlocks, Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-28 09:14:36 UTC (rev 7309) +++ code/stage/trunk/libstage/model.cc 2009-01-29 23:52:51 UTC (rev 7310) @@ -1784,6 +1784,8 @@ { if( child->parent ) child->parent->RemoveChild( child ); + else + world->RemoveChild( child ); child->parent = this; Added: code/stage/trunk/libstage/puck.cc =================================================================== --- code/stage/trunk/libstage/puck.cc (rev 0) +++ code/stage/trunk/libstage/puck.cc 2009-01-29 23:52:51 UTC (rev 7310) @@ -0,0 +1,75 @@ + +#include "stage.hh" +#include "worldfile.hh" +using namespace Stg; + +Puck::Puck() : + color( stg_color_pack( 1,0,0,0) ), + displaylist( 0 ), + height( 0.2 ), + pose( 0,0,0,0 ), + radius( 0.1 ) +{ /* empty */ } + +void Puck::Load( Worldfile* wf, int section ) +{ + pose.Load( wf, section, "pose" ); + //color.Load( wf, section, "color" ); + + radius = wf->ReadLength( section, "radius", radius ); + height = wf->ReadLength( section, "height", height ); + + BuildDisplayList(); +} + +void Puck::Save( Worldfile* wf, int section ) +{ + pose.Save( wf, section, "pose" ); + //color.Save( wf, section, "color" ); + + wf->WriteLength( section, "radius", radius ); + wf->WriteLength( section, "height", height ); +} + +void Puck::BuildDisplayList() +{ + if( displaylist == 0 ) + displaylist = glGenLists(1); + + glNewList( displaylist, GL_COMPILE ); + + GLUquadricObj *q = gluNewQuadric(); + glColor3f( 1,0,0 ); + + // draw a filled body + glPushMatrix(); + glTranslatef( pose.x, pose.y, pose.z ); + gluCylinder(q, radius, radius, height, 16, 1 ); + glTranslatef( 0,0, height); + gluDisk(q, 0, radius, 16, 16 ); + glPopMatrix(); + + // outline the sides in a darker shade + double r,g,b,a; + stg_color_unpack( color, &r, &g, &b, &a ); + glColor3f( r/2.0, g/2.0, b/2.0 ); + + gluQuadricDrawStyle( q, GLU_LINE ); + glTranslatef( pose.x, pose.y, pose.z ); + gluCylinder(q, radius+0.001, radius+0.001, height+0.001, 16, 1 ); + + // clean up + gluDeleteQuadric( q ); + glEndList(); +} + + +void Puck::Draw() +{ + if( displaylist == 0 ) + BuildDisplayList(); + + glPushMatrix(); + glCallList( displaylist ); + glPopMatrix(); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-28 09:14:36 UTC (rev 7309) +++ code/stage/trunk/libstage/stage.hh 2009-01-29 23:52:51 UTC (rev 7310) @@ -213,7 +213,7 @@ /** Obtain the components of a color */ void stg_color_unpack( stg_color_t col, - double* r, double* g, double* b, double* a ); + double* r, double* g, double* b, double* a ); /** specify a rectangular size */ class Size @@ -222,8 +222,8 @@ stg_meters_t x, y, z; Size( stg_meters_t x, - stg_meters_t y, - stg_meters_t z ) + stg_meters_t y, + stg_meters_t z ) : x(x), y(y), z(z) {/*empty*/} @@ -243,9 +243,9 @@ stg_radians_t a;///< rotation about the z axis. Pose( stg_meters_t x, - stg_meters_t y, - stg_meters_t z, - stg_radians_t a ) + stg_meters_t y, + stg_meters_t z, + stg_radians_t a ) : x(x), y(y), z(z), a(a) { /*empty*/ } @@ -255,14 +255,14 @@ virtual ~Pose(){}; /** return a random pose within the bounding rectangle, with z=0 and - angle random */ + angle random */ static Pose Random( stg_meters_t xmin, stg_meters_t xmax, - stg_meters_t ymin, stg_meters_t ymax ) + stg_meters_t ymin, stg_meters_t ymax ) { return Pose( xmin + drand48() * (xmax-xmin), - ymin + drand48() * (ymax-ymin), - 0, - normalize( drand48() * (2.0 * M_PI) )); + ymin + drand48() * (ymax-ymin), + 0, + normalize( drand48() * (2.0 * M_PI) )); } /** Print pose in human-readable format on stdout @@ -271,7 +271,7 @@ virtual void Print( const char* prefix ) { printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", - prefix, x,y,z,a ); + prefix, x,y,z,a ); } void Load( Worldfile* wf, int section, const char* keyword ); @@ -289,9 +289,9 @@ @param a heading in radians */ Velocity( stg_meters_t x, - stg_meters_t y, - stg_meters_t z, - stg_radians_t a ) + stg_meters_t y, + stg_meters_t z, + stg_radians_t a ) { /*empty*/ } Velocity() @@ -303,7 +303,7 @@ virtual void Print( const char* prefix ) { printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", - prefix, x,y,z,a ); + prefix, x,y,z,a ); } }; @@ -318,12 +318,12 @@ void Print( const char* prefix ) { printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n", - prefix, - pose.x, - pose.y, - pose.a, - size.x, - size.y ); + prefix, + pose.x, + pose.y, + pose.a, + size.x, + size.y ); } }; @@ -538,8 +538,8 @@ /** Create a draw_t object of specified type from a vertex array */ draw_t* create( type_t type, - vertex_t* verts, - size_t vert_count ); + vertex_t* verts, + size_t vert_count ); /** Delete the draw_t object, deallocting its memory */ void destroy( draw_t* d ); @@ -587,8 +587,8 @@ extern stg_typetable_entry_t typetable[MODEL_TYPE_COUNT]; void RegisterModel( stg_model_type_t type, - const char* name, - stg_creator_t creator ); + const char* name, + stg_creator_t creator ); void RegisterModels(); @@ -655,17 +655,17 @@ the caller must free [rects]. */ int stg_rotrects_from_image_file( const char* filename, - stg_rotrect_t** rects, - unsigned int* rect_count, - unsigned int* widthp, - unsigned int* heightp ); + stg_rotrect_t** rects, + unsigned int* rect_count, + unsigned int* widthp, + unsigned int* heightp ); /** matching function should return true iff the candidate block is stops the ray, false if the block transmits the ray */ typedef bool (*stg_ray_test_func_t)(Model* candidate, - Model* finder, - const void* arg ); + Model* finder, + const void* arg ); // TODO - some of this needs to be implemented, the rest junked. @@ -945,22 +945,22 @@ inline Cell* GetCell( const int32_t x, const int32_t y ); void ForEachCellInPolygon( const stg_point_t pts[], - const uint32_t pt_count, - stg_cell_callback_t cb, - void* cb_arg ); + const uint32_t pt_count, + stg_cell_callback_t cb, + void* cb_arg ); void ForEachCellInLine( const stg_point_t pt1, - const stg_point_t pt2, - stg_cell_callback_t cb, - void* cb_arg ); + const stg_point_t pt2, + stg_cell_callback_t cb, + void* cb_arg ); void ForEachCellInLine( stg_meters_t x1, stg_meters_t y1, - stg_meters_t x2, stg_meters_t y2, - stg_cell_callback_t cb, - void* cb_arg ); + stg_meters_t x2, stg_meters_t y2, + stg_cell_callback_t cb, + void* cb_arg ); /** convert a distance in meters to a distance in world occupancy - grid pixels */ + grid pixels */ int32_t MetersToPixels( stg_meters_t x ) { return (int32_t)floor(x * ppm); }; @@ -973,21 +973,21 @@ void DestroySuperRegion( SuperRegion* sr ); stg_raytrace_result_t Raytrace( const Pose& pose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const Model* finder, - const void* arg, - const bool ztest ); + const stg_meters_t range, + const stg_ray_test_func_t func, + const Model* finder, + const void* arg, + const bool ztest ); void Raytrace( const Pose &pose, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const Model* finder, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest ); + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const Model* finder, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest ); /** Enlarge the bounding volume to include this point */ @@ -1003,7 +1003,7 @@ void RecordRay( double x1, double y1, double x2, double y2 ); /** Returns true iff the current time is greater than the time we - should quit */ + should quit */ bool PastQuitTime(); void StartUpdatingModel( Model* mod ) @@ -1025,8 +1025,8 @@ static bool UpdateAll(); World( const char* token = "MyWorld", - stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM, - double ppm = DEFAULT_PPM ); + stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM, + double ppm = DEFAULT_PPM ); virtual ~World(); @@ -1055,11 +1055,11 @@ void TryCharge( PowerPack* pp, Pose pose ); /** Get the resolution in pixels-per-metre of the underlying - discrete raytracing model */ + discrete raytracing model */ double Resolution(){ return ppm; }; /** Returns a pointer to the model identified by name, or NULL if - nonexistent */ + nonexistent */ Model* GetModel( const char* name ); /** Return the 3D bounding box of the world, in meters */ @@ -1079,15 +1079,15 @@ public: /** Block Constructor. A model's body is a list of these - blocks. The point data is copied, so pts can safely be freed - after constructing the block.*/ + 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, - stg_color_t color, - bool inherit_color ); + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + stg_color_t color, + bool inherit_color ); /** A from-file constructor */ Block( Model* mod, Worldfile* wf, int entity); @@ -1153,15 +1153,15 @@ bool mapped; /** an array of pointers to cells into which this block has been - rendered (speeds up UnMapping) */ + rendered (speeds up UnMapping) */ GPtrArray* rendered_cells; /** When moving a model, we test for collisions by generating, for - each block, a list of the cells in which it would be rendered if the - move were to be successful. If no collision occurs, the move is - allowed - the rendered cells are cleared, the potential cells are - written, and the pointers to the rendered and potential cells are - switched for next time (avoiding a memory copy).*/ + each block, a list of the cells in which it would be rendered if the + move were to be successful. If no collision occurs, the move is + allowed - the rendered cells are cleared, the potential cells are + written, and the pointers to the rendered and potential cells are + switched for next time (avoiding a memory copy).*/ GPtrArray* candidate_cells; }; @@ -1190,14 +1190,14 @@ stg_point3_t GetOffset(){ return offset; }; /** establish the min and max of all the blocks, so we can scale this - group later */ + group later */ void CalcSize(); void AppendBlock( Block* block ); void CallDisplayList( Model* mod ); void Clear() ; /** deletes all blocks from the group */ /** Returns a pointer to the first model detected to be colliding - with a block in this group, or NULL, if none are detected. */ + with a block in this group, or NULL, if none are detected. */ Model* TestCollision(); void SwitchToTestedCells(); @@ -1324,9 +1324,9 @@ inline void addPitch( float pitch ) { _pitch += pitch; if( _pitch > 90 ) - _pitch = 90; + _pitch = 90; else if( _pitch < 0 ) - _pitch = 0; + _pitch = 0; } inline void setScale( float scale ) { _scale = scale; } @@ -1416,1226 +1416,1240 @@ void Show(); /** Get human readable string that describes the current simulation - time. */ + time. */ std::string ClockString( void ); /** Set the minimum real time interval between world updates, in - microeconds. */ + microeconds. */ void SetRealTimeInterval( stg_usec_t usec ) { interval_real = usec; } + + virtual void RemoveChild( Model* mod ); }; -class Camera; + class Camera; -/** energy data packet */ -class PowerPack -{ -public: - PowerPack( Model* mod ); + /** energy data packet */ + class PowerPack + { + public: + PowerPack( Model* mod ); - /** The model that owns this object */ - Model* mod; + /** The model that owns this object */ + Model* mod; - /** Energy stored */ - stg_joules_t stored; + /** Energy stored */ + stg_joules_t stored; - /** Energy capacity */ - stg_joules_t capacity; + /** Energy capacity */ + stg_joules_t capacity; - /** TRUE iff the device is receiving energy from a charger */ - bool charging; + /** TRUE iff the device is receiving energy from a charger */ + bool charging; - /** OpenGL visualization of the powerpack state */ - void Visualize( Camera* cam ); + /** OpenGL visualization of the powerpack state */ + void Visualize( Camera* cam ); - /** Print human-readable status on stdout, prefixed with the - argument string */ - void Print( char* prefix ); -}; + /** Print human-readable status on stdout, prefixed with the + argument string */ + void Print( char* prefix ); + }; -class Visibility -{ -public: - bool blob_return; - int fiducial_key; - int fiducial_return; - bool gripper_return; - stg_laser_return_t laser_return; - bool obstacle_return; - bool ranger_return; + class Visibility + { + public: + bool blob_return; + int fiducial_key; + int fiducial_return; + bool gripper_return; + stg_laser_return_t laser_return; + bool obstacle_return; + bool ranger_return; - Visibility(); - void Load( Worldfile* wf, int wf_entity ); -}; + Visibility(); + void Load( Worldfile* wf, int wf_entity ); + }; -/** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ -class CustomVisualizer { -public: - //TODO allow user to specify name - which will show up in display filter - virtual ~CustomVisualizer( void ) { } - virtual void DataVisualize( Camera* cam ) = 0; - virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory) -}; + /** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ + class CustomVisualizer { + public: + //TODO allow user to specify name - which will show up in display filter + virtual ~CustomVisualizer( void ) { } + virtual void DataVisualize( Camera* cam ) = 0; + virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory) + }; -/* Hooks for attaching special callback functions (not used as - variables - we just need unique addresses for them.) */ -class CallbackHooks -{ -public: - char load; - char save; - char shutdown; - char startup; - char update; -}; + /* Hooks for attaching special callback functions (not used as + variables - we just need unique addresses for them.) */ + class CallbackHooks + { + public: + char load; + char save; + char shutdown; + char startup; + char update; + }; -/** Records model state and functionality in the GUI, if used */ -class GuiState -{ -public: - bool grid; - unsigned int mask; - bool nose; - bool outline; + /** Records model state and functionality in the GUI, if used */ + class GuiState + { + public: + bool grid; + unsigned int mask; + bool nose; + bool outline; - GuiState(); - void Load( Worldfile* wf, int wf_entity ); -}; + GuiState(); + void Load( Worldfile* wf, int wf_entity ); + }; -// class Option { -// private: -// friend bool compare( const Option* lhs, const Option* rhs ); + // class Option { + // private: + // friend bool compare( const Option* lhs, const Option* rhs ); -// std::string optName; -// bool value; -// /** worldfile entry string for loading and saving this value */ -// std::string wf_token; -// std::string shortcut; -// Fl_Menu_* menu; -// int menuIndex; -// Fl_Callback* menuCb; -// Fl_Widget* menuCbWidget; + // std::string optName; + // bool value; + // /** worldfile entry string for loading and saving this value */ + // std::string wf_token; + // std::string shortcut; + // Fl_Menu_* menu; + // int menuIndex; + // Fl_Callback* menuCb; + // Fl_Widget* menuCbWidget; -// public: -// Option( std::string n, std::string tok, std::string key, bool v, WorldGui* worldgui ); + // public: + // Option( std::string n, std::string tok, std::string key, bool v, WorldGui* worldgui ); -// const std::string name() const { return optName; } -// inline bool val() const { return value; } -// inline operator bool() { return val(); } -// inline bool operator<( const Option& rhs ) const -// { return optName<rhs.optName; } -// void set( bool val ); -// void invert() { set( !value ); } + // const std::string name() const { return optName; } + // inline bool val() const { return value; } + // inline operator bool() { return val(); } + // inline bool operator<( const Option& rhs ) const + // { return optName<rhs.optName; } + // void set( bool val ); + // void invert() { set( !value ); } -// // Comparator to dereference Option pointers and compare their strings -// struct optComp { -// inline bool operator()( const Option* lhs, const Option* rhs ) const -// { return lhs->operator<(*rhs); } -// }; + // // Comparator to dereference Option pointers and compare their strings + // struct optComp { + // inline bool operator()( const Option* lhs, const Option* rhs ) const + // { return lhs->operator<(*rhs); } + // }; -// void createMenuItem( Fl_Menu_Bar* menu, std::string path ); -// void menuCallback( Fl_Callback* cb, Fl_Widget* w ); -// static void toggleCb( Fl_Widget* w, void* p ); -// void Load( Worldfile* wf, int section ); -// void Save( Worldfile* wf, int section ); -// }; + // void createMenuItem( Fl_Menu_Bar* menu, std::string path ); + // void menuCallback( Fl_Callback* cb, Fl_Widget* w ); + // static void toggleCb( Fl_Widget* w, void* p ); + // void Load( Worldfile* wf, int section ); + // void Save( Worldfile* wf, int section ); + // }; -/// %Model class -class Model : public Ancestor -{ - friend class Ancestor; - friend class World; - friend class WorldGui; - friend class Canvas; - friend class Block; - friend class Region; - friend class BlockGroup; + /// %Model class + class Model : public Ancestor + { + friend class Ancestor; + friend class World; + friend class WorldGui; + friend class Canvas; + friend class Block; + friend class Region; + friend class BlockGroup; -private: - /** the number of models instatiated - used to assign unique IDs */ - static uint32_t count; - static GHashTable* modelsbyid; - std::vector<Option*> drawOptions; - const std::vector<Option*>& getOptions() const { return drawOptions; } + private: + /** the number of models instatiated - used to assign unique IDs */ + static uint32_t count; + static GHashTable* modelsbyid; + std::vector<Option*> drawOptions; + const std::vector<Option*>& getOptions() const { return drawOptions; } -protected: - GMutex* access_mutex; - GPtrArray* blinkenlights; - BlockGroup blockgroup; - /** OpenGL display list identifier for the blockgroup */ - int blocks_dl; + protected: + GMutex* access_mutex; + GPtrArray* blinkenlights; + BlockGroup blockgroup; + /** OpenGL display list identifier for the blockgroup */ + int blocks_dl; - /** Iff true, 4 thin blocks are automatically added to the model, - forming a solid boundary around the bounding box of the - model. */ - int boundary; + /** Iff true, 4 thin blocks are automatically added to the model, + forming a solid boundary around the bounding box of the + model. */ + int boundary; - /** callback functions can be attached to any field in this - structure. When the internal function model_change(<mod>,<field>) - is called, the callback is triggered */ - GHashTable* callbacks; + /** callback functions can be attached to any field in this + structure. When the internal function model_change(<mod>,<field>) + is called, the callback is triggered */ + GHashTable* callbacks; - /** Default color of the model's blocks.*/ - stg_color_t color; + /** Default color of the model's blocks.*/ + stg_color_t color; - /** This can be set to indicate that the model has new data that - may be of interest to users. This allows polling the model - instead of adding a data callback. */ - bool data_fresh; - stg_bool_t disabled; ///< if non-zero, the model is disabled - GList* custom_visual_list; - GList* flag_list; - Geom geom; - Pose global_pose; - bool gpose_dirty; ///< set this to indicate that global pose may have changed - /** Controls our appearance and functionality in the GUI, if used */ - GuiState gui; + /** This can be set to indicate that the model has new data that + may be of interest to users. This allows polling the model + instead of adding a data callback. */ + bool data_fresh; + stg_bool_t disabled; ///< if non-zero, the model is disabled + GList* custom_visual_list; + GList* flag_list; + Geom geom; + Pose global_pose; + bool gpose_dirty; ///< set this to indicate that global pose may have changed + /** Controls our appearance and functionality in the GUI, if used */ + GuiState gui; - bool has_default_block; + bool has_default_block; - /* hooks for attaching special callback functions (not used as - variables - we just need unique addresses for them.) */ - CallbackHooks hooks; + /* hooks for attaching special callback functions (not used as + variables - we just need unique addresses for them.) */ + CallbackHooks hooks; - /** unique process-wide identifier for this model */ - uint32_t id; - ctrlinit_t* initfunc; - stg_usec_t interval; ///< time between updates in us - stg_usec_t last_update; ///< time of last update in us - bool map_caches_are_invalid; - stg_meters_t map_resolution; - stg_kg_t mass; - bool on_update_list; - bool on_velocity_list; + /** unique process-wide identifier for this model */ + uint32_t id; + ctrlinit_t* initfunc; + stg_usec_t interval; ///< time between updates in us + stg_usec_t last_update; ///< time of last update in us + bool map_caches_are_invalid; + stg_meters_t map_resolution; + stg_kg_t mass; + bool on_update_list; + bool on_velocity_list; - /** Pointer to the parent of this model, possibly NULL. */ - Model* parent; + /** Pointer to the parent of this model, possibly NULL. */ + Model* parent; - /** The pose of the model in it's parents coordinate frame, or the - global coordinate frame is the parent is NULL. */ - Pose pose; + /** The pose of the model in it's parents coordinate frame, or the + global coordinate frame is the parent is NULL. */ + Pose pose; - /** GData datalist can contain arbitrary named data items. Can be used - by derived model types to store properties, and for user code - to associate arbitrary items with a model. */ - GData* props; - bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw - char* say_string; ///< if non-null, this string is displayed in the GUI + /** GData datalist can contain arbitrary named data items. Can be used + by derived model types to store properties, and for user code + to associate arbitrary items with a model. */ + GData* props; + bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw + char* say_string; ///< if non-null, this string is displayed in the GUI - stg_bool_t stall; - /** Thread safety flag. Iff true, Update() may be called in - parallel with other models. Defaults to false for safety */ - int subs; ///< the number of subscriptions to this model - bool thread_safe; - GArray* trail; - stg_model_type_t type; - bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType() - Velocity velocity; - stg_watts_t watts;///< power consumed by this model - Worldfile* wf; - int wf_entity; - World* world; // pointer to the world in which this model exists - WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode + stg_bool_t stall; + /** Thread safety flag. Iff true, Update() may be called in + parallel with other models. Defaults to false for safety */ + int subs; ///< the number of subscriptions to this model + bool thread_safe; + GArray* trail; + stg_model_type_t type; + bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType() + Velocity velocity; + stg_watts_t watts;///< power consumed by this model + Worldfile* wf; + int wf_entity; + World* world; // pointer to the world in which this model exists + WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode -public: + public: - Visibility vis; + Visibility vis; - /** Optional attached PowerPack, defaults to NULL */ - PowerPack* power_pack; + /** Optional attached PowerPack, defaults to NULL */ + PowerPack* power_pack; - void Lock() - { - if( access_mutex == NULL ) - access_mutex = g_mutex_new(); + void Lock() + { + if( access_mutex == NULL ) + access_mutex = g_mutex_new(); - assert( access_mutex ); - g_mutex_lock( access_mutex ); - } + assert( access_mutex ); + g_mutex_lock( access_mutex ); + } - void Unlock() - { - assert( access_mutex ); - g_mutex_unlock( access_mutex ); - } + void Unlock() + { + assert( access_mutex ); + g_mutex_unlock( access_mutex ); + } -private: - /** Private copy constructor declared but not defined, to make it - impossible to copy models. */ - explicit Model(const Model& original); + private: + /** Private copy constructor declared but not defined, to make it + impossible to copy models. */ + explicit Model(const Model& original); - /** Private assignment operator declared but not defined, to make it - impossible to copy models */ - Model& operator=(const Model& original); + /** Private assignment operator declared but not defined, to make it + impossible to copy models */ + Model& operator=(const Model& original); -protected: + protected: - /// Register an Option for pickup by the GUI - void registerOption( Option* opt ) - { drawOptions.push_back( opt ); } + /// Register an Option for pickup by the GUI + void registerOption( Option* opt ) + { drawOptions.push_back( opt ); } - /** Check to see if the current pose will yield a collision with - obstacles. Returns a pointer to the first entity we are in - collision with, or NULL if no collision exists. */ - Model* TestCollision(); + /** Check to see if the current pose will yield a collision with + obstacles. Returns a pointer to the first entity we are in + collision with, or NULL if no collision exists. */ + Model* TestCollision(); - void CommitTestedPose(); + void CommitTestedPose(); - void Map(); - void UnMap(); + void Map(); + void UnMap(); - void MapWithChildren(); - void UnMapWithChildren(); + void MapWithChildren(); + void UnMapWithChildren(); - int TreeToPtrArray( GPtrArray* array ); + int TreeToPtrArray( GPtrArray* array ); - /** raytraces a single ray from the point and heading identified by - pose, in local coords */ - stg_raytrace_result_t Raytrace( const Pose &pose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); + /** raytraces a single ray from the point and heading identified by + pose, in local coords */ + stg_raytrace_result_t Raytrace( const Pose &pose, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest = true ); - /** raytraces multiple rays around the point and heading identified - by pose, in local coords */ - void Raytrace( const Pose &pose, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); + /** raytraces multiple rays around the point and heading identified + by pose, in local coords */ + void Raytrace( const Pose &pose, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest = true ); - stg_raytrace_result_t Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); + stg_raytrace_result_t Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_ray_test_func_t func, + const void* arg, + const bool ztest = true ); - void Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); + void Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const void* arg, + stg_raytrace_result_t* samples, + const uint32_t sample_count, + const bool ztest = true ); - /** Causes this model and its children to recompute their global - position instead of using a cached pose in - Model::GetGlobalPose()..*/ - void GPoseDirtyTree(); + /** Causes this model and its children to recompute their global + position instead of using a cached pose in + Model::GetGlobalPose()..*/ + void GPoseDirtyTree(); - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void UpdatePose(); + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void UpdatePose(); - void StartUpdating(); - void StopUpdating(); + void StartUpdating(); + void StopUpdating(); - Model* ConditionalMove( Pose newpose ); + Model* ConditionalMove( Pose newpose ); - stg_meters_t ModelHeight(); + stg_meters_t ModelHeight(); - bool UpdateDue( void ); - void UpdateIfDue(); + bool UpdateDue( void ); + void UpdateIfDue(); - void DrawBlocksTree(); - virtual void DrawBlocks(); - void DrawBoundingBox(); - void DrawBoundingBoxTree(); - virtual void DrawStatus( Camera* cam ); - void DrawStatusTree( Camera* cam ); + void DrawBlocksTree(); + virtual void DrawBlocks(); + void DrawBoundingBox(); + void DrawBoundingBoxTree(); + virtual void DrawStatus( Camera* cam ); + void DrawStatusTree( Camera* cam ); - void DrawOriginTree(); - void DrawOrigin(); + void DrawOriginTree(); + void DrawOrigin(); - void PushLocalCoords(); - void PopCoords(); + void PushLocalCoords(); + void PopCoords(); - /** Draw the image stored in texture_id above the model */ - void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); + /** Draw the image stored in texture_id above the model */ + void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); - /** static wrapper for DrawBlocks() */ - static void DrawBlocks( gpointer dummykey, - Model* mod, - void* arg ); + /** static wrapper for DrawBlocks() */ + static void DrawBlocks( gpointer dummykey, + Model* mod, + void* arg ); - virtual void DrawPicker(); - virtual void DataVisualize( Camera* cam ); + virtual void DrawPicker(); + virtual void DataVisualize( Camera* cam ); - virtual void DrawSelected(void); + virtual void DrawSelected(void); - void DrawTrailFootprint(); - void DrawTrailBlocks(); - void DrawTrailArrows(); - void DrawGrid(); + void DrawTrailFootprint(); + void DrawTrailBlocks(); + void DrawTrailArrows(); + void DrawGrid(); - void DrawBlinkenlights(); + void DrawBlinkenlights(); - void DataVisualizeTree( Camera* cam ); + void DataVisualizeTree( Camera* cam ); - virtual void PushColor( stg_color_t col ) - { world->PushColor( col ); } + virtual void PushColor( stg_color_t col ) + { world->PushColor( col ); } - virtual void PushColor( double r, double g, double b, double a ) - { world->PushColor( r,g,b,a ); } + virtual void PushColor( double r, double g, double b, double a ) + { world->PushColor( r,g,b,a ); } - virtual void PopColor(){ world->PopColor(); } + virtual void PopColor(){ world->PopColor(); } - void DrawFlagList(); + void DrawFlagList(); - void DrawPose( Pose pose ); + void DrawPose( Pose pose ); -public: - void RecordRenderPoint( GSList** head, GSList* link, - unsigned int* c1, unsigned int* c2 ); + public: + void RecordRenderPoint( GSList** head, GSList* link, + unsigned int* c1, unsigned int* c2 ); - void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, - stg_meters_t ymin, stg_meters_t ymax ); + void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, + stg_meters_t ymin, stg_meters_t ymax ); - /** Look up a model pointer by a unique model ID */ - static Model* LookupId( uint32_t id ) - { return (Model*)g_hash_table_lookup( modelsbyid, (void*)id ); } + /** Look up a model pointer by a unique model ID */ + static Model* LookupId( uint32_t id ) + { return (Model*)g_hash_table_lookup( modelsbyid, (void*)id ); } - /** Constructor */ - Model( World* world, - Model* parent, - stg_model_type_t type = MODEL_TYPE_PLAIN ); + /** Constructor */ + Model( World* world, + Model* parent, + stg_model_type_t type = MODEL_TYPE_PLAIN ); - /** Destructor */ - virtual ~Model(); + /** Destructor */ + virtual ~Model(); - void Say( const char* str ); - /** Attach a user supplied visualization to a model */ - void AddCustomVisualizer( CustomVisualizer* custom_visual ); - /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ - void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); + void Say( const char* str ); + /** Attach a user supplied visualization to a model */ + void AddCustomVisualizer( CustomVisualizer* custom_visual ); + /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ + void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); - void BecomeParentOf( Model* child ); + void BecomeParentOf( Model* child ); - void Load( Worldfile* wf, int wf_entity ) - { - /** Set the worldfile and worldfile entity ID - must be called - before Load() */ - SetWorldfile( wf, wf_entity ); - Load(); // call virtual load - } + void Load( Worldfile* wf, int wf_entity ) + { + /** Set the worldfile and worldfile entity ID - must be called + before Load() */ + SetWorldfile( wf, wf_entity ); + Load(); // call virtual load + } - /** Set the worldfile and worldfile entity ID */ - void SetWorldfile( Worldfile* wf, int wf_entity ) - { this->wf = wf; this->wf_entity = wf_entity; } + /** Set the worldfile and worldfile entity ID */ + void SetWorldfile( Worldfile* wf, int wf_entity ) + { this->wf = wf; this->wf_entity = wf_entity; } - /** configure a model by reading from the current world file */ - virtual void Load(); + /** configure a model by reading from the current world file */ + virtual void Load(); - /** save the state of the model to the current world file */ - virtual void Save(); + /** save the state of the model to the current world file */ + virtual void Save(); - /** Should be called after all models are loaded, to do any last-minute setup */ - void Init(); - void InitRecursive(); + /** Should be called after all models are loaded, to do any last-minute setup */ + void Init(); + void InitRecursive(); - void AddFlag( Flag* flag ); - void RemoveFlag( Flag* flag ); + void AddFlag( Flag* flag ); + void RemoveFlag( Flag* flag ); - void PushFlag( Flag* flag ); - Flag* PopFlag(); + void PushFlag( Flag* flag ); + Flag* PopFlag(); - int GetFlagCount(){ return g_list_length( flag_list ); } + int GetFlagCount(){ return g_list_length( flag_list ); } - /** Add a pointer to a blinkenlight to the model. */ - void AddBlinkenlight( stg_blinkenlight_t* b ) - { g_ptr_array_add( this->blinkenlights, b ); } + /** Add a pointer to a blinkenlight to the model. */ + void AddBlinkenlight( stg_blinkenlight_t* b ) + { g_ptr_array_add( this->blinkenlights, b ); } - /** Clear all blinkenlights from the model. Does not destroy the - blinkenlight objects. */ - void ClearBlinkenlights() - { g_ptr_array_set_size( this->blinkenlights, 0 ); } + /** Clear all blinkenlights from the model. Does not destroy the + blinkenlight objects. */ + void ClearBlinkenlights() + { g_ptr_array_set_size( this->blinkenlights, 0 ); } - /** Disable the model. Its pose will not change due to velocity - until re-enabled using Enable(). This is used for example when - dragging a model with the mouse pointer. The model is enabled by - default. */ - void Disable(){ disabled = true; }; + /** Disable the model. Its pose will not change due to velocity + until re-enabled using Enable(). This is used for example when + dragging a model with the mouse pointer. The model is enabled by + default. */ + void Disable(){ disabled = true; }; - /** Enable the model, so that non-zero velocities will change the - model's pose. Models are enabled by default. */ - void Enable(){ disabled = false; }; + /** Enable the model, so that non-zero velocities will change the + model's pose. Models are enabled by default. */ + void Enable(){ disabled = false; }; - /** Load a control program for this model from an external - library */ - void LoadControllerModule( char* lib ); + /** Load a control program for this model from an external + library */ + void LoadControllerModule( char* lib ); - /** Sets the redraw flag, so this model will be redrawn at the - earliest opportunity */ - void NeedRedraw(); + /** Sets the redraw flag, so this model will be redrawn at the + earliest opportunity */ + void NeedRedraw(); - /** Add a block to this model by loading it from a worldfile - entity */ - void LoadBlock( Worldfile* wf, int entity ); + /** Add a block to this model by loading it from a worldfile + entity */ + void LoadBlock( Worldfile* wf, int entity ); - /** Add a block to this model centered at [x,y] with extent [dx, dy, - dz] */ - void AddBlockRect( stg_meters_t x, stg_meters_t y, - stg_meters_t dx, stg_meters_t dy, - stg_meters_t dz ); + /** Add a block to this model centered at [x,y] with extent [dx, dy, + dz] */ + void AddBlockRect( stg_meters_t x, stg_meters_t y, + stg_meters_t dx, stg_meters_t dy, + stg_meters_t dz ); - /** remove all blocks from this model, freeing their memory */ - void ClearBlocks(); + /** remove all blocks from this model, freeing their memory */ + void ClearBlocks(); - /** Returns a pointer to this model's parent model, or NULL if this - model has no parent */ - Model* Parent(){ return this->parent; } + /** Returns a pointer to this model's parent model, or NULL if this + model has no parent */ + Model* Parent(){ return this->parent; } - Model* GetModel( const char* name ); - //int GuiMask(){ return this->gui_mask; }; + Model* GetModel( const char* name ); + //int GuiMask(){ return this->gui_mask; }; - /** Returns a pointer to the world that contains this model */ - World* GetWorld(){ return this->world; } + /** Returns a pointer to the world that contains this model */ + World* GetWorld(){ return this->world; } - /** return the root model of the tree containing this model */ - Model* Root(){ return( parent ? parent->Root() : this ); } + /** return the root model of the tree containing this model */ + Model* Root(){ return( parent ? parent->Root() : this ); } - bool IsAntecedent( Model* testmod ); + bool IsAntecedent( Model* testmod ); - /** returns true if model [testmod] is a descendent of this model */ - bool IsDescendent( Model* testmod ); + /** returns true if model [testmod] is a descendent of this model */ + bool IsDescendent( Model* testmod ); - /** returns true if model [testmod] is a descendent or antecedent of this model */ - bool IsRelated( Model* testmod ); + /** returns true if model [testmod] is a descendent or antecedent of this model */ + bool IsRelated( Model* testmod ); - /** get the pose of a model in the global CS */ - Pose GetGlobalPose(); + /** get the pose of a model in the global CS */ + Pose GetGlobalPose(); - /** get the velocity of a model in the global CS */ - Velocity GetGlobalVelocity(); + /** get the velocity of a model in the global CS */ + Velocity GetGlobalVelocity(); - /* set the velocity of a model in the global coordinate system */ - void SetGlobalVelocity( Velocity gvel ); + /* set the velocity of a model in the global coordinate system */ + void SetGlobalVelocity( Velocity gvel ); - /** subscribe to a model's data */ - void Subscribe(); + /** subscribe to a model's data */ + void Subscribe(); - /** unsubscribe from a model's data */ - void Unsubscribe(); + /** unsubscribe from a model's data */ + void Unsubscribe(); - /** set the pose of model in global coordinates */ - void SetGlobalPose( Pose gpose ); + /** set the pose of model in global coordinates */ + void SetGlobalPose( Pose gpose ); - /** set a model's velocity in its parent's coordinate system */ - void SetVelocity( Velocity vel ); + /** set a model's velocity in its parent's coordinate system */ + void SetVelocity( Velocity vel ); - /** set a model's pose in its parent's coordinate system */ - void SetPose( Pose pose ); + /** set a model's pose in its parent's coordinate system */ + void SetPose( Pose pose ); - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( Pose pose ); + /** add values to a model's pose in its parent's coordinate system */ + void AddToPose( Pose pose ); - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( double dx, double dy, double dz, double da ); + /** add values to a model's pose in its parent's coordinate system */ + void AddToPose( double dx, double dy, double dz, double da ); - /** set a model's geometry (size and center offsets) */ - void SetGeom( Geom src ); + /** set a model's geometry (size and center offsets) */ + void SetGeom( Geom src ); - /** Set a model's fiducial return value. Values less than zero - are not detected by the fiducial sensor. */ - void SetFiducialReturn( int fid ); + /** Set a model's fiducial return value. Values less than zero + are not detected by the fiducial sensor. */ + void SetFiducialReturn( int fid ); - /** Get a model's fiducial return value. */ - int GetFiducialReturn() - { return vis.fiducial_return; } + /** Get a model's fiducial return value. */ + int GetFiducialReturn() + { return vis.fiducial_return; } - /** set a model's fiducial key: only fiducial finders with a - matching key can detect this model as a fiducial. */ - void SetFiducialKey( int key ); + /** set a model's fiducial key: only fiducial finders with a + matching key can detect this model as a fiducial. */ + void SetFiducialKey( int key ); - stg_color_t GetColor(){ return color; } + stg_color_t GetColor(){ return color; } - // stg_laser_return_t GetLaserReturn(){ return laser_return; } + // stg_laser_return_t GetLaserReturn(){ return laser_return; } - /** Change a model's parent - experimental*/ - int SetParent( Model* newparent); + /** Change a model's parent - experimental*/ + int SetParent( Model* newparent); - /** Get a model's geometry - it's size and local pose (offset from - origin in local coords) */ - Geom GetGeom(){ return geom; } + /** Get a model's geometry - it's size and local pose (offset from + origin in local coords) */ + Geom GetGeom(){ return geom; } - /** Get the pose of a model in its parent's coordinate system */ - Pose GetPose(){ return pose; } + /** Get the pose of a model in its parent's coordinate system */ + Pose GetPose(){ return pose; } - /** Get a model's velocity (in its local reference frame) */ - Velocity GetVelocity(){ return velocity; } + /** Get a model's velocity (in its local reference frame) */ + Velocity GetVelocity(){ return velocity; } - // guess what these do? - void SetColor( stg_color_t col ); - void SetMass( stg_kg_t mass ); - void SetStall( stg_bool_t stall ); - void SetGripperReturn( int val ); - void SetLaserReturn( stg_laser_return_t val ); - void SetObstacleReturn( int val ); - void SetBlobReturn( int val ); - void SetRangerReturn( int val ); - void SetBoundary( int val ); - void SetGuiNose( int val ); - void SetGuiMask( int val ); - void SetGuiGrid( int val ); - void SetGuiOutline( int val ); - void SetWatts( stg_watts_t watts ); - void SetMapResolution( stg_meters_t res ); + // guess what these do? + void SetColor( stg_color_t col ); + void SetMass( stg_kg_t mass ); + void SetStall( stg_bool_t stall ); + void SetGripperReturn( int val ); + void SetLaserReturn( stg_laser_return_t val ); + void SetObstacleReturn( int val ); + void SetBlobReturn( int val ); + void SetRangerReturn( int val ); + void SetBoundary( int val ); + void SetGuiNose( int val ); + void SetGuiMask( int val ); + void SetGuiGrid( int val ); + void SetGuiOutline( int val ); + void SetWatts( stg_watts_t watts ); + void SetMapResolution( stg_meters_t res ); - bool DataIsFresh(){ return this->data_fresh; } + bool DataIsFresh(){ return this->data_fresh; } - /* attach callback functions to data members. The function gets - called when the member is changed using SetX() accessor method */ + /* attach callback functions to data members. The function gets + called when the member is changed using SetX() accessor method */ - void AddCallback( void* address, - stg_model_callback_t cb, - void* user ); + void AddCallback( void* address, + stg_model_callback_t cb, + void* user ); - int RemoveCallback( void* member, - stg_model_callback_t callback ); + int RemoveCallback( void* member, + stg_model_callback_t callback ); - void CallCallbacks( void* address ); + void CallCallbacks( void* address ); - /* wrappers for the generic callback add & remove functions that hide - some implementation detail */ + /* wrappers for the generic callback add & remove functions that hide + some implementation detail */ - void AddStartupCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.startup, cb, user ); }; + void AddStartupCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.startup, cb, user ); }; - void RemoveStartupCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.startup, cb ); }; + void RemoveStartupCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.startup, cb ); }; - void AddShutdownCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.shutdown, cb, user ); }; + void AddShutdownCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.shutdown, cb, user ); }; - void RemoveShutdownCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.shutdown, cb ); } + void RemoveShutdownCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.shutdown, cb ); } - void AddLoadCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.load, cb, user ); } + void AddLoadCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.load, cb, user ); } - void RemoveLoadCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.load, cb ); } + void RemoveLoadCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.load, cb ); } - void AddSaveCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.save, cb, user ); } + void AddSaveCallback( stg_model_callback_t cb, void* user ) + { AddCallback( &hooks.save, cb, user ); } - void RemoveSaveCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.save, cb ); } + void RemoveSaveCallback( stg_model_callback_t cb ) + { RemoveCallback( &hooks.save, cb ); } - void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { - AddCallback( &hooks.update, cb, user ); - Subscribe(); // if attaching a callback here, assume we want updates to happen - } + void AddUpdateCallback( stg_model_callback_t cb, void* user ) + { + AddCallback( &hooks.update, cb, user ); + Subscribe(); // if attaching a callback here, assume we want updates to happen + } - void RemoveUpdateCallback( stg_model_callback_t cb ) - { - RemoveCallback( &hooks.update, cb ); - Unsubscribe(); - } + void RemoveUpdateCallback( stg_model_callback_t cb ) + { + RemoveCallback( &hooks.update, cb ); + Unsubscribe(); + } - /** named-property interface - */ - void* GetProperty( char* key ); + /** named-property interface + */ + void* GetProperty( char* key ); - /** @brief Set a named property of a Stage model. + /** @brief Set a named property of a Stage model. - Set a property of a Stage model. + Set a property of a Stage model. - This function can set both predefined and user-defined - properties of a model. Predefined properties are intrinsic to - every model, such as pose and color. Every supported predefined - properties has its identifying string defined as a preprocessor - macro in stage.h. Users should use the macro instead of a - hard-coded string, so that the compiler can help you to avoid - mis-naming properties. + This function can set both predefined and user-defined + properties of a model. Predefined properties are intrinsic to + every model, such as pose and color. Every supported predefined + properties has its identifying string defined as a preprocessor + macro in stage.h. Users should use the macro instead of a + hard-coded string, so that the compiler can help you to avoid + mis-naming properties. - User-defined properties allow the user to attach arbitrary data - pointers to a model. User-defined property data is not copied, - so the original pointer must remain valid. User-defined property - names are simple strings. Names beginning with an underscore - ('_') are reserved for internal libstage use: users should not - use names beginning with underscore (at risk of causing very - weird behaviour). + User-defined properties allow the user to attach arbitrary data + pointers to a model. User-defined property data is not copied, + so the original pointer must remain valid. User-defined property + names are simple strings. Names beginning with an underscore + ('_') are reserved for internal libstage use: users should not + use names beginning with underscore (at risk of causing very + weird behaviour). - Any callbacks registered for the named property will be called. + Any callbacks registered for the named property will be called. - Returns 0 on success, or a positive error code on failure. + Returns 0 on success, or a positive error code on failure. - *CAUTION* The caller is responsible for making sure the pointer - points to data of the correct type for the property, so use - carefully. Check the docs or the equivalent - stg_model_set_<property>() function definition to see the type - of data required for each property. - */ - int SetProperty( char* key, void* data ); - void UnsetProperty( char* key ); + *CAUTION* The caller is responsible for making sure the pointer + points to data of the correct type for the property, so use + carefully. Check the docs or the equivalent + stg_model_set_<property>() function definition to see the type + of data required for each property. + */ + int SetProperty( char* key, void* data ); + void UnsetProperty( char* key ); - virtual void Print( char* prefix ); - virtual const char* PrintWithPose(); + virtual void Print( char* prefix ); + virtual const char* PrintWithPose(); - /** Convert a pose in the world coordinate system into a model's - local coordinate system. Overwrites [pose] with the new - coordinate. */ - Pose GlobalToLocal( const Pose pose ); + /** Convert a pose in the world coordinate system into a model's + local coordinate system. Overwrites [pose] with the new + coordinate. */ + Pose GlobalToLocal( const Pose pose ); - /** 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 ); + /** 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 ); - /** 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 ); + /** 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 ); - /** returns the first descendent of this model that is unsubscribed - and has the type indicated by the string */ - Model* GetUnsubscribedModelOfType( const stg_model_type_t type ); + /** returns the first descendent of this model that is unsubscribed + and has the type indicated by the string */ + Model* GetUnsubscribedModelOfType( const stg_model_type_t type ); - /** returns the first descendent of this model that is unused - and has the type indicated by the string. This model is tagged as used. */ - Model* GetUnusedModelOfType( const stg_model_type_t type ); + /** returns the first descendent of this model that is unused + and has the type indicated by the string. This model is tagged as used. */ + Model* GetUnusedModelOfType( const stg_model_type_t type ); - /** Returns the value of the model's stall boolean, which is true - iff the model has crashed into another model */ - bool Stalled(){ return this->stall; } -}; + /** Returns the value of the model's stall boolean, which is true + iff the model has crashed into another model */ + bool Stalled(){ return this->stall; } + }; -// BLOBFINDER MODEL -------------------------------------------------------- + // BLOBFINDER MODEL -------------------------------------------------------- -/** blobfinder data packet - */ -typedef struct -{ - stg_color_t color; - uint32_t left, top, right, bottom; - stg_meters_t range; -} stg_blobfinder_blob_t; + /** blobfinder data packet + */ + typedef struct + { + stg_color_t color; + uint32_t left, top, right, bottom; + stg_meters_t range; + } stg_blobfinder_blob_t; -/// %ModelBlobfinder class -class ModelBlobfinder : public Model -{ -private: - GArray* colors; - GArray* blobs; + /// %ModelBlobfinder class + class ModelBlobfinder : public Model + { + private: + GArray* colors; + GArray* blobs; - // predicate for ray tracing - static bool BlockMatcher( Block* testblock, Model* finder ); + // predicate for ray tracing + static bool BlockMatcher( Block* testblock, Model* finder ); - static Option showBlobData; + static Option showBlobData; - virtual void DataVisualize( Camera* cam ); + virtual void DataVisualize( Camera* cam ); -public: - unsigned int scan_width; - unsigned int scan_height; - stg_meters_t range; - stg_radians_t fov; - stg_radians_t pan; + public: + unsigned int scan_width; + unsigned int scan_height; + stg_meters_t range; + stg_radians_t fov; + stg_radians_t pan; - static const char* typestr; + static const char* typestr; - // constructor - ModelBlobfinder( World* world, - Model* parent ); - // destructor - ~ModelBlobfinder(); + // constructor + ModelBlobfinder( World* world, + Model* parent ); + // destructor + ~ModelBlobfinder(); - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void Load(); + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Load(); - stg_blobfinder_blob_t* GetBlobs( unsi... [truncated message content] |
From: <rt...@us...> - 2009-01-30 03:47:41
|
Revision: 7312 http://playerstage.svn.sourceforge.net/playerstage/?rev=7312&view=rev Author: rtv Date: 2009-01-30 03:47:38 +0000 (Fri, 30 Jan 2009) Log Message: ----------- added contact recharging between power packs. Removed charger object Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/charger.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Added Paths: ----------- code/stage/trunk/assets/mains.png Added: code/stage/trunk/assets/mains.png =================================================================== (Binary files differ) Property changes on: code/stage/trunk/assets/mains.png ___________________________________________________________________ Added: svn:mime-type + image/png Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -88,9 +88,9 @@ // inspect the laser data and decide what to do int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) { - if( laser->power_pack && laser->power_pack->charging ) - printf( "model %s power pack @%p is charging\n", - laser->Token(), laser->power_pack ); +// if( laser->power_pack && laser->power_pack->charging ) +// printf( "model %s power pack @%p is charging\n", +// laser->Token(), laser->power_pack ); // Get the data uint32_t sample_count=0; Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-30 03:47:38 UTC (rev 7312) @@ -8,7 +8,6 @@ blockgroup.cc camera.cc canvas.cc - charger.cc file_manager.cc file_manager.hh gl.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/canvas.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -133,12 +133,22 @@ std::string fullpath = FileManager::findFile( "assets/stall.png" ); if ( fullpath == "" ) { - PRINT_DEBUG( "Unable to load texture.\n" ); + PRINT_DEBUG( "Unable to load stall texture.\n" ); } GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); TextureManager::getInstance()._stall_texture_id = stall_id; + + fullpath = FileManager::findFile( "assets/mains.png" ); + if ( fullpath == "" ) + { + PRINT_DEBUG( "Unable to load mains texture.\n" ); + } + GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); + TextureManager::getInstance()._mains_texture_id = mains_id; + + //TODO merge this code into the textureManager? int i, j; for (i = 0; i < checkImageHeight; i++) @@ -923,9 +933,6 @@ for( GList* it=selected_models; it; it=it->next ) ((Model*)it->data)->DrawSelected(); - for( GList* it=world->chargers; it; it=it->next ) - ((Charger*)it->data)->Visualize(); - // useful debug - puts a point at the origin of each model //for( GList* it = world->World::children; it; it=it->next ) // ((Model*)it->data)->DrawOriginTree(); Modified: code/stage/trunk/libstage/charger.cc =================================================================== --- code/stage/trunk/libstage/charger.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/charger.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -80,7 +80,7 @@ glPopMatrix(); // ? - glPolygonMode( GL_FRONT, GL_FILL ); + //glPolygonMode( GL_FRONT, GL_FILL ); } void swap( double &a, double &b ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/model.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -234,7 +234,9 @@ type(type), used(false), velocity(), - watts(0), + watts(0.0), + watts_give(0.0), + watts_take(0.0), wf(NULL), wf_entity(0), world(world), @@ -743,31 +745,27 @@ // this->world->sim_time, this->token, this->subs ); // f we're drawing current and a power pack has been installed - if( power_pack ) + + PowerPack* pp = FindPowerPack(); + if( pp && ( watts > 0 )) { - if( watts > 0 ) - { - // consume energy stored in the power pack - stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - power_pack->stored -= consumed; - - /* - printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", - token, - watts, - consumed, - power_pack, - power_pack->stored, - power_pack->capacity, - power_pack->stored / power_pack->capacity * 100.0 ); - */ - } + // consume energy stored in the power pack + stg_joules_t consumed = watts * (world->interval_sim * 1e-6); + pp->Subtract( consumed ); - // I own this power pack, see if the world wants to recharge it */ - if( power_pack->mod == this ) - world->TryCharge( power_pack, GetGlobalPose() ); + /* + printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", + token, + watts, + consumed, + power_pack, + power_pack->stored, + power_pack->capacity, + power_pack->stored / power_pack->capacity * 100.0 ); + */ + } - + CallCallbacks( &hooks.update ); last_update = world->sim_time; } @@ -1051,7 +1049,9 @@ void Model::DrawStatus( Camera* cam ) { - + // quick hack + if( power_pack && power_pack->stored < 0.0 ) + DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 ); if( say_string || power_pack ) { @@ -1076,7 +1076,7 @@ //if( ! parent ) // glRectf( 0,0,1,1 ); - if( power_pack && (power_pack->mod == this) ) + if( power_pack->stored > 0.0 ) power_pack->Visualize( cam ); if( say_string ) @@ -1204,6 +1204,8 @@ PushLocalCoords(); + glPolygonMode( GL_FRONT, GL_FILL ); + GLUquadric* quadric = gluNewQuadric(); glTranslatef(0,0,1); // jump up Pose gpose = GetGlobalPose(); @@ -1606,17 +1608,37 @@ Model* Model::TestCollision() { //printf( "mod %s test collision...\n", token ); - + Model* hitmod = blockgroup.TestCollision(); if( hitmod == NULL ) for( GList* it = children; it; it=it->next ) { - hitmod = ((Model*)it->data)->TestCollision(); - if( hitmod ) - break; + hitmod = ((Model*)it->data)->TestCollision(); + if( hitmod ) + break; } + if( hitmod && (watts_take > 0.0) ) + { + PowerPack* pp = FindPowerPack(); + + if( pp ) + { + if( hitmod->FindPowerPack() && (hitmod->watts_give > 0.0) ) + { + stg_watts_t rate = MIN( watts_take, hitmod->watts_give ); + stg_joules_t amount = rate * (world->interval_sim * 1e-6); + + // move some joules from him to me + hitmod->FindPowerPack()->TransferTo( FindPowerPack(), amount ); + } + else + pp->charging = false; + } + } + + //printf( "mod %s test collision done.\n", token ); return hitmod; } @@ -1793,3 +1815,14 @@ world->dirty = true; } + +PowerPack* Model::FindPowerPack() +{ + if( power_pack ) + return power_pack; + + if( parent ) + return parent->FindPowerPack(); + + return NULL; +} Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/model_load.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -18,8 +18,9 @@ { assert( wf ); assert( wf_entity ); - + PRINT_DEBUG1( "Model \"%s\" loading...", token ); + if( wf->PropertyExists( wf_entity, "joules" ) ) { @@ -33,17 +34,16 @@ the charge */ power_pack->capacity = power_pack->stored; } - + if( wf->PropertyExists( wf_entity, "joules_capacity" ) ) { if( !power_pack ) power_pack = new PowerPack( this ); power_pack->capacity = - wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity ); - + wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity ); } - + /** if the capacity has been specified, limit the store to the capacity */ if( power_pack && (power_pack->stored > power_pack->capacity) ) { @@ -53,26 +53,22 @@ power_pack->stored, power_pack->capacity ); } + + // use my own pack or an ancestor's for the other energy properties + PowerPack* pp = FindPowerPack(); - if( wf->PropertyExists( wf_entity, "watts" ) ) - { - watts = wf->ReadFloat( wf_entity, "watts", watts ); - - if( watts > 0 ) - { - // find a power pack attached to me or an ancestor in my tree - while( (!power_pack) && parent ) - { - power_pack = parent->power_pack; - } - - if( power_pack == NULL ) - { - PRINT_WARN2( "worldfile requests %.2f watts for model %s, but can not find an energy source. Setting watts has no effect unless you also specify a \"joules\" value for this model or an ancestor.", watts, token ); - exit(-1); - } - } - } + watts = wf->ReadFloat( wf_entity, "watts", watts ); + if( (watts > 0) && !pp ) + PRINT_WARN1( "Model %s: Setting \"watts\" has no effect unless \"joules\" is specified for this model or a parent", token ); + + watts_give = wf->ReadFloat( wf_entity, "give_watts", watts_give ); + if( (watts_give > 0.0) && !pp) + PRINT_WARN1( "Model %s: Setting \"watts_give\" has no effect unless \"joules\" is specified for this model or a parent", token ); + + watts_take = wf->ReadFloat( wf_entity, "take_watts", watts_take ); + if( (watts_take > 0.0) & !pp ) + PRINT_WARN1( "Model %s: Setting \"watts_take\" has no effect unless \"joules\" is specified for this model or a parent", token ); + if( wf->PropertyExists( wf_entity, "debug" ) ) { Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/powerpack.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -6,6 +6,7 @@ */ #include "stage.hh" +#include "texture_manager.hh" using namespace Stg; PowerPack::PowerPack( Model* mod ) : @@ -25,52 +26,89 @@ { const double height = 0.5; const double width = 0.2; - + double percent = stored/capacity * 100.0; + + if( percent > 50 ) + glColor4f( 0,1,0, 0.7 ); // green + else if( percent > 25 ) + glColor4f( 1,0,1, 0.7 ); // magenta + else + glColor4f( 1,0,0, 0.7 ); // red + + static char buf[6]; + snprintf( buf, 6, "%.0f", percent ); + + glTranslatef( -width, 0.0, 0.0 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + GLfloat fullness = height * (percent * 0.01); + glRectf( 0,0,width, fullness); + + // outline the charge-o-meter + glTranslatef( 0,0,0.001 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + glColor4f( 0,0,0,0.7 ); + + glRectf( 0,0,width, height ); + + glBegin( GL_LINES ); + glVertex2f( 0, fullness ); + glVertex2f( width, fullness ); + glEnd(); + + if( charging ) + { + glLineWidth( 6.0 ); + glColor4f( 1,0,0,0.7 ); + + glRectf( 0,0,width, height ); + + glLineWidth( 1.0 ); + } - if( percent > 50 ) - glColor4f( 0,1,0, 0.7 ); // green - else if( percent > 25 ) - glColor4f( 1,0,1, 0.7 ); // magenta - else - glColor4f( 1,0,0, 0.7 ); // red - - static char buf[6]; - snprintf( buf, 6, "%.0f", percent ); + // draw the percentage + //gl_draw_string( -0.2, 0, 0, buf ); + + // ? + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); +} - glTranslatef( -width, 0.0, 0.0 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); +stg_joules_t PowerPack::RemainingCapacity() +{ + return( capacity - stored ); +} - GLfloat fullness = height * (percent * 0.01); - glRectf( 0,0,width, fullness); +void PowerPack::Add( stg_joules_t j ) +{ + stored += MIN( RemainingCapacity(), j ); + + charging = true; +} - // outline the charge-o-meter - glTranslatef( 0,0,0.001 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); +void PowerPack::Subtract( stg_joules_t j ) +{ + if( stored >= 0.0 ) + stored -= MIN( stored, j ); +} - glColor4f( 0,0,0,0.7 ); +void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount ) +{ + // if stored is non-negative we can't transfer more than the stored + // amount. If it is negative, we have infinite energy stored + if( stored >= 0.0 ) + amount = MIN( stored, amount ); - glRectf( 0,0,width, height ); + // we can't transfer more than he can take + amount = MIN( amount, dest->RemainingCapacity() ); + + printf( "%s receives %.3f J from %s\n", + mod->Token(), amount, dest->mod->Token() ); - glBegin( GL_LINES ); - glVertex2f( 0, fullness ); - glVertex2f( width, fullness ); - glEnd(); - - if( charging ) - { - glLineWidth( 6.0 ); - glColor4f( 1,0,0,0.7 ); - - glRectf( 0,0,width, height ); - - glLineWidth( 1.0 ); - } - - // draw the percentage - //gl_draw_string( -0.2, 0, 0, buf ); - - // ? - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + Subtract( amount ); + dest->Add( amount ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/stage.hh 2009-01-30 03:47:38 UTC (rev 7312) @@ -666,62 +666,7 @@ typedef bool (*stg_ray_test_func_t)(Model* candidate, Model* finder, const void* arg ); - - // TODO - some of this needs to be implemented, the rest junked. - /* // -------------------------------------------------------------- */ - - /* // standard energy consumption of some devices in W. */ - /* // */ - /* // The MOTIONKG value is a hack to approximate cost of motion, as */ - /* // Stage does not yet have an acceleration model. */ - /* // */ - /* #define STG_ENERGY_COST_LASER 20.0 // 20 Watts! (LMS200 - from SICK web site) */ - /* #define STG_ENERGY_COST_FIDUCIAL 10.0 // 10 Watts */ - /* #define STG_ENERGY_COST_RANGER 0.5 // 500mW (estimate) */ - /* #define STG_ENERGY_COST_MOTIONKG 10.0 // 10 Watts per KG when moving */ - /* #define STG_ENERGY_COST_BLOB 4.0 // 4W (estimate) */ - - /* typedef struct */ - /* { */ - /* stg_joules_t joules; // current energy stored in Joules/1000 */ - /* stg_watts_t watts; // current power expenditure in mW (mJoules/sec) */ - /* int charging; // 1 if we are receiving energy, -1 if we are */ - /* // supplying energy, 0 if we are neither charging nor */ - /* // supplying energy. */ - /* stg_meters_t range; // the range that our charging probe hit a charger */ - /* } stg_energy_data_t; */ - - /* typedef struct */ - /* { */ - /* stg_joules_t capacity; // maximum energy we can store (we start fully charged) */ - /* stg_meters_t probe_range; // the length of our recharge probe */ - /* //Pose probe_pose; // TODO - the origin of our probe */ - - /* stg_watts_t give_rate; // give this many Watts to a probe that hits me (possibly 0) */ - - /* stg_watts_t trickle_rate; // this much energy is consumed or */ - /* // received by this device per second as a */ - /* // baseline trickle. Positive values mean */ - /* // that the device is just burning energy */ - /* // stayting alive, which is appropriate */ - /* // for most devices. Negative values mean */ - /* // that the device is receiving energy */ - /* // from the environment, simulating a */ - /* // solar cell or some other ambient energy */ - /* // collector */ - - /* } stg_energy_config_t; */ - - - /* // BLINKENLIGHT ------------------------------------------------------------ */ - - /* // a number of milliseconds, used for example as the blinkenlight interval */ - /* #define STG_LIGHT_ON UINT_MAX */ - /* #define STG_LIGHT_OFF 0 */ - - //typedef int stg_interval_ms_t; - // list iterator macros #define LISTFUNCTION( LIST, TYPE, FUNC ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data); #define LISTMETHOD( LIST, TYPE, METHOD ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD(); @@ -860,22 +805,6 @@ class BlockGroup; class PowerPack; - /// %Charger class - class Charger - { - World* world; - stg_watts_t watts; - stg_bounds3d_t volume; - - public: - Charger( World* world ); - void ChargeIfContained( PowerPack* pp, Pose pose ); - bool Contains( Pose pose ); - void Charge( PowerPack* pp ); - void Visualize(); - void Load( Worldfile* wf, int entity ); - }; - /// %World class class World : public Ancestor { @@ -883,7 +812,6 @@ friend class Block; //friend class StgTime; friend class Canvas; - friend class Charger; private: @@ -892,7 +820,6 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - GList* chargers; bool destroy; bool dirty; ///< iff true, a gui redraw would be required GHashTable* models_by_name; ///< the models that make up the world, indexed by name @@ -935,7 +862,6 @@ void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ); - void LoadCharger( Worldfile* wf, int entity ); SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); @@ -1445,7 +1371,7 @@ /** Energy capacity */ stg_joules_t capacity; - /** TRUE iff the device is receiving energy from a charger */ + /** TRUE iff the device is receiving energy */ bool charging; /** OpenGL visualization of the powerpack state */ @@ -1454,7 +1380,19 @@ /** Print human-readable status on stdout, prefixed with the argument string */ void Print( char* prefix ); - }; + + /** Returns the energy capacity minus the current amount stored */ + stg_joules_t RemainingCapacity(); + + /** Add to the energy store */ + void Add( stg_joules_t j ); + + /** Subtract from the energy store */ + void Subtract( stg_joules_t j ); + + /** Transfer some stored energy to another power pack */ + void TransferTo( PowerPack* dest, stg_joules_t amount ); +}; class Visibility { @@ -1557,7 +1495,8 @@ friend class Block; friend class Region; friend class BlockGroup; - + friend class PowerPack; + private: /** the number of models instatiated - used to assign unique IDs */ static uint32_t count; @@ -1584,7 +1523,7 @@ /** Default color of the model's blocks.*/ stg_color_t color; - + /** This can be set to indicate that the model has new data that may be of interest to users. This allows polling the model instead of adding a data callback. */ @@ -1622,6 +1561,8 @@ global coordinate frame is the parent is NULL. */ Pose pose; + /** Optional attached PowerPack, defaults to NULL */ + PowerPack* power_pack; /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code @@ -1640,6 +1581,15 @@ bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType() Velocity velocity; stg_watts_t watts;///< power consumed by this model + + /** If >0, this model can transfer energy to models that have + watts_take >0 */ + stg_watts_t watts_give; + + /** If >0, this model can transfer energy from models that have + watts_give >0 */ + stg_watts_t watts_take; + Worldfile* wf; int wf_entity; World* world; // pointer to the world in which this model exists @@ -1649,8 +1599,6 @@ Visibility vis; - /** Optional attached PowerPack, defaults to NULL */ - PowerPack* power_pack; void Lock() { @@ -1801,8 +1749,10 @@ void DrawFlagList(); void DrawPose( Pose pose ); - + public: + PowerPack* FindPowerPack(); + void RecordRenderPoint( GSList** head, GSList* link, unsigned int* c1, unsigned int* c2 ); Modified: code/stage/trunk/libstage/texture_manager.hh =================================================================== --- code/stage/trunk/libstage/texture_manager.hh 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/texture_manager.hh 2009-01-30 03:47:38 UTC (rev 7312) @@ -28,6 +28,7 @@ //TODO figure out where to store standard textures GLuint _stall_texture_id; + GLuint _mains_texture_id; //TODO make this threadsafe inline static TextureManager& getInstance( void ) { Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/libstage/world.cc 2009-01-30 03:47:38 UTC (rev 7312) @@ -73,7 +73,6 @@ double ppm ) : // private - chargers( NULL ), destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), @@ -338,8 +337,6 @@ LoadBlock( wf, entity, entitytable ); else if( strcmp( typestr, "puck" ) == 0 ) LoadPuck( wf, entity, entitytable ); - else if( strcmp( typestr, "charger" ) == 0 ) - LoadCharger( wf, entity ); else LoadModel( wf, entity, entitytable ); } @@ -362,16 +359,6 @@ putchar( '\n' ); } -void World::LoadCharger( Worldfile* wf, int entity ) -{ - Charger* chg = new Charger( this ); - - chargers = g_list_prepend( chargers, chg ); - - chg->Load( wf, entity ); -} - - // delete a model from the hash table static void destroy_model( gpointer dummy1, Model* mod, gpointer dummy2 ) { @@ -945,16 +932,5 @@ extent.z.max = MAX( extent.z.max, pt.z ); } -void World::TryCharge( PowerPack* pack, Pose pose ) -{ - pack->charging = false; - // see if the pose lies within any of the charging rectangles - for( GList* it = chargers; it; it = it->next ) - { - Charger* chg = (Charger*)it->data; - chg->ChargeIfContained( pack, pose ); - } -} - Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-29 23:58:07 UTC (rev 7311) +++ code/stage/trunk/worlds/fasr.world 2009-01-30 03:47:38 UTC (rev 7312) @@ -22,9 +22,9 @@ ( size [ 600.000 599.000 ] - center [ 0.051 -0.204 ] + center [ 4.605 -2.392 ] rotate [ 0 0 ] - scale 33.822 + scale 68.242 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -45,18 +45,6 @@ bitmap "bitmaps/cave.png" ) -charger -( - volume [ 5 6 -2 -3 0 0.1 ] - watts 200 -) - -charger -( - volume [ 5 6 -4 -5 0 0.1 ] - watts 200 -) - zone ( color "green" @@ -73,11 +61,6 @@ ctrl "sink" ) -#puck( pose [ 0 0 0 0 ] ) -#puck( pose [ 1 0 0 0 ] ) -#puck( pose [ 2 0 0 0 ] ) -#puck( pose [ 3 0 0 0 ] ) - define autorob pioneer2dx ( sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) @@ -86,9 +69,44 @@ fiducial_return 1 ) -autorob( pose [-1.768 2.263 0 -147.323] fiducial( range_max 3 ) ) -#autorob( pose [4.116 6.107 0 -147.323] ) -autorob( pose [6.471 5.304 0 14.941] ) +define charging_bump model +( + pose [0.210 0 -0.100 0 ] + size [0.050 0.100 0.100] + take_watts 300.0 + color "orange" + obstacle_return 1 +) + +define charge_station model +( + color "yellow" + size [ 0.100 0.500 0.400 ] + joules -1 # infinite storage + give_watts 500 +) + +charge_station +( + pose [ 7.941 -3.001 0 0 ] +) + +autorob +( + pose [2.933 0.917 0 -147.323] + color "magenta" + joules 10000 + joules_capacity 100000 + charging_bump() +) + +autorob +( + pose [5.757 -3.528 0 14.941] + color "green" + charging_bump( take_watts 0 give_watts 300.0 ) +) + autorob( pose [5.937 4.858 0 -147.503] ) autorob( pose [7.574 6.269 0 -111.715] ) autorob( pose [5.664 5.938 0 107.666] ) @@ -103,7 +121,7 @@ autorob( pose [5.098 6.788 0 -61.295] ) autorob( pose [4.374 5.163 0 -147.713] ) autorob( pose [4.999 4.230 0 -125.236] ) -autorob( pose [-3.326 0.642 0 78.789] ) +autorob( pose [3.533 4.220 0 78.789] ) autorob( pose [5.440 5.317 0 -26.545] ) autorob( pose [7.518 6.973 0 163.239] ) autorob( pose [7.559 4.764 0 -139.066] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-07 03:22:24
|
Revision: 7319 http://playerstage.svn.sourceforge.net/playerstage/?rev=7319&view=rev Author: rtv Date: 2009-02-07 02:17:30 +0000 (Sat, 07 Feb 2009) Log Message: ----------- working on charging model and demo Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/examples/ctrl/source.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -11,39 +11,68 @@ const double stopdist = 0.5; const int avoidduration = 10; const int workduration = 20; -const int payload = 4; +const int payload = 1; double have[4][4] = { + // { -120, -180, 180, 180 }, + //{ -90, -120, 180, 90 }, { 90, 180, 180, 180 }, - { 90, -90, 0, -90 }, + { 90, -90, 180, 90 }, { 90, 90, 180, 90 }, { 0, 45, 0, 0} }; double need[4][4] = { { -120, -180, 180, 180 }, - { -90, -120, 180, 180 }, + { -90, -120, 180, 90 }, { -90, -90, 180, 180 }, { -90, -180, -90, -90 } }; +double refuel[4][4] = { + { 0, 0, 45, 120 }, + { 0,-90, -60, -160 }, + { -90, -90, 180, 180 }, + { -90, -180, -90, -90 } +}; +typedef enum { + MODE_WORK=0, + MODE_DOCK, + MODE_UNDOCK +} nav_mode_t; + class Robot { private: ModelPosition* pos; ModelLaser* laser; ModelRanger* ranger; - //ModelBlobfinder* blobfinder; ModelFiducial* fiducial; + ModelBlobfinder* blobfinder; Model *source, *sink; int avoidcount, randcount; int work_get, work_put; - + bool charger_ahoy; + double charger_bearing; + double charger_range; + double charger_heading; + nav_mode_t mode; + static int LaserUpdate( ModelLaser* mod, Robot* robot ); static int PositionUpdate( ModelPosition* mod, Robot* robot ); static int FiducialUpdate( ModelFiducial* mod, Robot* robot ); + static int BlobFinderUpdate( ModelBlobfinder* mod, Robot* robot ); + + void Dock(); + void Work(); + void UnDock(); + bool ObstacleAvoid(); + // predicate that indicates if we need to charge + bool Hungry(); + bool Full(); + public: Robot( ModelPosition* pos, Model* source, @@ -51,25 +80,40 @@ : pos(pos), laser( (ModelLaser*)pos->GetUnusedModelOfType( MODEL_TYPE_LASER )), ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )), - fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )), + fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )), + blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )), source(source), sink(sink), avoidcount(0), randcount(0), work_get(0), - work_put(0) + work_put(0), + charger_ahoy(false), + charger_bearing(0), + charger_range(0), + charger_heading(0), + mode(MODE_WORK) { // need at least these models to get any work done // (pos must be good, as we used it in the initialization list) assert( laser ); assert( source ); assert( sink ); + + // PositionUpdate() checks to see if we reached source or sink + pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); - pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); + // LaserUpdate() controls the robot, by reading from laser and + // writing to position laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this ); + // trivial demos + if( fiducial ) // optional fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); + + if( blobfinder ) // optional + blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); } }; @@ -85,36 +129,85 @@ return 0; //ok } -// inspect the laser data and decide what to do -int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) + + +void Robot::Dock() { -// if( laser->power_pack && laser->power_pack->charging ) -// printf( "model %s power pack @%p is charging\n", -// laser->Token(), laser->power_pack ); - - // Get the data - uint32_t sample_count=0; - stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); + if( charger_ahoy ) + { + double a_goal = normalize( charger_bearing ); + +// if( pos->Stalled() ) +// { +// puts( "stalled. stopping" ); +// pos->Stop(); +// } +// else - if( scan == NULL ) - return 0; - + if( charger_range > 0.5 ) + { + if( !ObstacleAvoid() ) + { + pos->SetXSpeed( cruisespeed ); + pos->SetTurnSpeed( a_goal ); + } + } + else + { + pos->SetTurnSpeed( a_goal ); + pos->SetXSpeed( 0.02 ); // creep towards it + + if( charger_range < 0.08 ) // close enough + pos->Stop(); + + if( pos->Stalled() ) // touching + pos->SetXSpeed( -0.01 ); // back off a bit + + } + } + else + { + //printf( "docking but can't see a charger\n" ); + pos->Stop(); + mode = MODE_WORK; // should get us back on track eventually + } + + // if the battery is charged, go back to work + if( Full() ) + { + //printf( "fully charged, now back to work\n" ); + mode = MODE_UNDOCK; + } +} + + +void Robot::UnDock() +{ + if( charger_range < 0.3 ) + pos->SetXSpeed( -0.05 ); + else + mode = MODE_WORK; +} + +bool Robot::ObstacleAvoid() +{ bool obstruction = false; bool stop = false; - + // find the closest distance to the left and right and check if // there's anything in front double minleft = 1e6; double minright = 1e6; - - //return 0; + // Get the data + uint32_t sample_count=0; + stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); + for (uint32_t i = 0; i < sample_count; i++) - { - + { if( verbose ) printf( "%.3f ", scan[i].range ); - - if( (i > (sample_count/4)) + + if( (i > (sample_count/4)) && (i < (sample_count - (sample_count/4))) && scan[i].range < minfrontdistance) { @@ -122,17 +215,17 @@ obstruction = true; } - if( scan[i].range < stopdist ) + if( scan[i].range < stopdist ) { if( verbose ) puts( " stopping!" ); stop = true; } - - if( i > sample_count/2 ) + + if( i > sample_count/2 ) minleft = MIN( minleft, scan[i].range ); - else + else minright = MIN( minright, scan[i].range ); - } + } if( verbose ) { @@ -140,71 +233,137 @@ printf( "minleft %.3f \n", minleft ); printf( "minright %.3f\n ", minright ); } - - if( obstruction || stop || (robot->avoidcount>0) ) - { - if( verbose ) printf( "Avoid %d\n", robot->avoidcount ); - - robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed ); - - /* once we start avoiding, select a turn direction and stick - with it for a few iterations */ - if( robot->avoidcount < 1 ) - { + + if( obstruction || stop || (avoidcount>0) ) + { + if( verbose ) printf( "Avoid %d\n", avoidcount ); + + pos->SetXSpeed( stop ? 0.0 : avoidspeed ); + + /* once we start avoiding, select a turn direction and stick + with it for a few iterations */ + if( avoidcount < 1 ) + { if( verbose ) puts( "Avoid START" ); - robot->avoidcount = random() % avoidduration + avoidduration; + avoidcount = random() % avoidduration + avoidduration; if( minleft < minright ) { - robot->pos->SetTurnSpeed( -avoidturn ); + pos->SetTurnSpeed( -avoidturn ); if( verbose ) printf( "turning right %.2f\n", -avoidturn ); } else { - robot->pos->SetTurnSpeed( +avoidturn ); + pos->SetTurnSpeed( +avoidturn ); if( verbose ) printf( "turning left %2f\n", +avoidturn ); } - } + } - robot->avoidcount--; - } - else - { - if( verbose ) puts( "Cruise" ); + avoidcount--; - robot->avoidcount = 0; - robot->pos->SetXSpeed( cruisespeed ); - - Pose pose = robot->pos->GetPose(); + return true; // busy avoding obstacles + } + + return false; // didn't have to avoid anything +} - int x = (pose.x + 8) / 4; - int y = (pose.y + 8) / 4; +void Robot::Work() +{ + if( ! ObstacleAvoid() ) + { + if( verbose ) puts( "Cruise" ); + + //avoidcount = 0; + pos->SetXSpeed( cruisespeed ); + + Pose pose = pos->GetPose(); + + int x = (pose.x + 8) / 4; + int y = (pose.y + 8) / 4; + // oh what an awful bug - 5 hours to track this down. When using // this controller in a world larger than 8*8 meters, a_goal can // sometimes be NAN. Causing trouble WAY upstream. - if( x > 3 ) x = 3; - if( y > 3 ) y = 3; - if( x < 0 ) x = 0; - if( y < 0 ) y = 0; - - double a_goal = - dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] ); - + if( x > 3 ) x = 3; + if( y > 3 ) y = 3; + if( x < 0 ) x = 0; + if( y < 0 ) y = 0; + + double a_goal = + dtor( pos->GetFlagCount() ? have[y][x] : need[y][x] ); + + // if we are low on juice - find the direction to the recharger instead + if( Hungry() ) + { + //puts( "hungry - using refuel map" ); + + // use the refuel map + a_goal = dtor( refuel[y][x] ); + + if( charger_ahoy ) // I see a charger while hungry! + mode = MODE_DOCK; + } + assert( ! isnan(a_goal ) ); assert( ! isnan(pose.a ) ); + + double a_error = normalize( a_goal - pose.a ); + + assert( ! isnan(a_error) ); + + pos->SetTurnSpeed( a_error ); + } +} - double a_error = normalize( a_goal - pose.a ); - assert( ! isnan(a_error) ); +// inspect the laser data and decide what to do +int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) +{ +// if( laser->power_pack && laser->power_pack->charging ) +// printf( "model %s power pack @%p is charging\n", +// laser->Token(), laser->power_pack ); + + if( laser->GetSamples(NULL) == NULL ) + return 0; - robot->pos->SetTurnSpeed( a_error ); - } - - - return 0; + switch( robot->mode ) + { + case MODE_DOCK: + //puts( "DOCK" ); + robot->Dock(); + break; + + case MODE_WORK: + //puts( "WORK" ); + robot->Work(); + break; + + case MODE_UNDOCK: + //puts( "UNDOCK" ); + robot->UnDock(); + break; + + default: + printf( "unrecognized mode %u\n", robot->mode ); + } + + //if( robot->charger_ahoy ) + //return 1; + //else + return 0; } +bool Robot::Hungry() +{ + return( pos->FindPowerPack()->ProportionRemaining() < 0.25 ); +} + +bool Robot::Full() +{ + return( pos->FindPowerPack()->ProportionRemaining() > 0.95 ); +} + int Robot::PositionUpdate( ModelPosition* pos, Robot* robot ) { Pose pose = pos->GetPose(); @@ -255,26 +414,40 @@ int Robot::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]; //printf( "fiducial %d is %d at %.2f m %.2f radians\n", - // i, f->id, f->range, f->bearing ); + // i, f->id, f->range, f->bearing ); - // if( 0 ) - if( f->range < 1 ) + if( f->id == 2 ) // I see a charging station { - printf( "attempt to grab model @%p %s\n", - f->mod, f->mod->Token() ); - - // working on picking up models - robot->pos->BecomeParentOf( f->mod ); - f->mod->SetPose( Pose(0,0,0,0) ); - f->mod->Disable(); + // 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; } - } - + return 0; // run again } + +int Robot::BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot ) +{ + unsigned int blob_count = 0; + stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count ); + + if( blobs && (blob_count>0) ) + { + printf( "%s sees %u blobs\n", blobmod->Token(), blob_count ); + } + + return 0; +} Modified: code/stage/trunk/examples/ctrl/source.cc =================================================================== --- code/stage/trunk/examples/ctrl/source.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/examples/ctrl/source.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -1,7 +1,7 @@ #include "stage.hh" using namespace Stg; -const int INTERVAL = 50; +const int INTERVAL = 200; int Update( Model* mod, void* dummy ); Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/block.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -22,7 +22,6 @@ inherit_color( inherit_color ), rendered_cells( g_ptr_array_sized_new(32) ), candidate_cells( g_ptr_array_sized_new(32) ) - // _gpts( NULL ) { assert( mod ); assert( pt_count > 0 ); @@ -30,9 +29,6 @@ local_z.min = zmin; local_z.max = zmax; - - // add this block's global coords array to a global list - //g_ptr_array_add( global_verts, this ); } /** A from-file constructor */ @@ -46,16 +42,12 @@ inherit_color(true), rendered_cells( g_ptr_array_sized_new(32) ), candidate_cells( g_ptr_array_sized_new(32) ) - // _gpts( NULL ) { assert(mod); assert(wf); assert(entity); Load( wf, entity ); - - // add this block's global coords array to a global list - //g_ptr_array_add( global_verts, this ); } @@ -67,9 +59,6 @@ g_ptr_array_free( rendered_cells, TRUE ); g_ptr_array_free( candidate_cells, TRUE ); - - //free( _gpts ); - //g_ptr_array_remove( global_verts, this ); } stg_color_t Block::GetColor() @@ -77,7 +66,27 @@ return( inherit_color ? mod->color : color ); } +GList* Block::AppendTouchingModels( GList* list ) +{ + // for every cell we are rendered into + for( unsigned int i=0; i<rendered_cells->len; i++ ) + { + Cell* cell = (Cell*)g_ptr_array_index( rendered_cells, i); + + // for every block rendered into that cell + for( GSList* it = cell->list; it; it=it->next ) + { + Block* testblock = (Block*)it->data; + Model* testmod = testblock->mod; + + if( !mod->IsRelated( testmod )) + if( ! g_list_find( list, testmod ) ) + list = g_list_append( list, testmod ); + } + } + return list; +} Model* Block::TestCollision() { @@ -86,8 +95,9 @@ // find the set of cells we would render into given the current global pose GenerateCandidateCells(); - // for every cell we may be rendered into - for( unsigned int i=0; i<candidate_cells->len; i++ ) + if( mod->vis.obstacle_return ) + // for every cell we may be rendered into + for( unsigned int i=0; i<candidate_cells->len; i++ ) { Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i); Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/blockgroup.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -50,6 +50,14 @@ LISTMETHOD( blocks, Block*, SwitchToTestedCells ); } +GList* BlockGroup::AppendTouchingModels( GList* list ) +{ + for( GList* it=blocks; it; it = it->next ) + list = ((Block*)it->data)->AppendTouchingModels( list ); + + return list; +} + Model* BlockGroup::TestCollision() { //printf( "blockgroup %p test collision...\n", this ); Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/canvas.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -139,7 +139,7 @@ GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); TextureManager::getInstance()._stall_texture_id = stall_id; - fullpath = FileManager::findFile( "assets/mains.png" ); + fullpath = FileManager::findFile( "assets/mainspower.png" ); if ( fullpath == "" ) { PRINT_DEBUG( "Unable to load mains texture.\n" ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/model.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -1050,13 +1050,13 @@ void Model::DrawStatus( Camera* cam ) { // quick hack - if( power_pack && power_pack->stored < 0.0 ) - { - glPushMatrix(); - glTranslatef( 0.3, 0, 0.0 ); - DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 ); - glPopMatrix(); - } +// if( power_pack && power_pack->stored < 0.0 ) +// { +// glPushMatrix(); +// glTranslatef( 0.3, 0, 0.0 ); +// DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 ); +// glPopMatrix(); +// } if( say_string || power_pack ) { @@ -1081,7 +1081,7 @@ //if( ! parent ) // glRectf( 0,0,1,1 ); - if( power_pack->stored > 0.0 ) + //if( power_pack->stored > 0.0 ) power_pack->Visualize( cam ); if( say_string ) @@ -1169,7 +1169,8 @@ return geom.size.z + m_child; } -void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha ) + +void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height ) { float yaw, pitch; pitch = - cam->pitch(); @@ -1192,9 +1193,9 @@ //draw a square, with the textured image glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f ); - glTexCoord2f(1.0f, 0.0f); glVertex3f( 0.25f, 0, -0.25f ); - glTexCoord2f(1.0f, 1.0f); glVertex3f( 0.25f, 0, 0.25f ); - glTexCoord2f(0.0f, 1.0f); glVertex3f(-0.25f, 0, 0.25f ); + glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f ); + glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f ); + glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f ); glEnd(); glPopMatrix(); @@ -1202,6 +1203,7 @@ glDisable(GL_TEXTURE_2D); } + void Model::DrawFlagList( void ) { if( flag_list == NULL ) @@ -1605,47 +1607,72 @@ void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, stg_meters_t ymin, stg_meters_t ymax ) { - while( TestCollision() ) + while( TestCollisionTree() ) SetPose( Pose::Random( xmin,xmax, ymin, ymax )); } +GList* Model::AppendTouchingModels( GList* list ) +{ + return blockgroup.AppendTouchingModels( list ); +} + Model* Model::TestCollision() { - //printf( "mod %s test collision...\n", token ); + //printf( "mod %s test collision...\n", token ); + return( blockgroup.TestCollision() ); +} + +Model* Model::TestCollisionTree() +{ + Model* hitmod = TestCollision(); - Model* hitmod = blockgroup.TestCollision(); - if( hitmod == NULL ) for( GList* it = children; it; it=it->next ) { - hitmod = ((Model*)it->data)->TestCollision(); + hitmod = ((Model*)it->data)->TestCollisionTree(); if( hitmod ) break; } - if( hitmod && (watts_take > 0.0) ) + //printf( "mod %s test collision done.\n", token ); + return hitmod; +} + +void Model::UpdateCharge() +{ + //printf( "model %s is a charger. testing to see if anyone is touching\n", + // token );x + + assert( watts_give > 0 ); + + PowerPack* mypp = FindPowerPack(); + assert( mypp ); + + for( GList* touchers = AppendTouchingModels( NULL ); + touchers; + touchers = touchers->next ) { - PowerPack* pp = FindPowerPack(); - - if( pp ) - { - if( hitmod->FindPowerPack() && (hitmod->watts_give > 0.0) ) - { - stg_watts_t rate = MIN( watts_take, hitmod->watts_give ); - stg_joules_t amount = rate * (world->interval_sim * 1e-6); - - // move some joules from him to me - hitmod->FindPowerPack()->TransferTo( FindPowerPack(), amount ); - } - else - pp->charging = false; - } - } - + Model* toucher = (Model*)touchers->data; + + PowerPack* hispp =toucher-> FindPowerPack(); + if( hispp && toucher->watts_take > 0.0) + { + //printf( " toucher %s can take up to %.2f wats\n", + // toucher->Token(), toucher->watts_take ); + + stg_watts_t rate = MIN( watts_give, toucher->watts_take ); + stg_joules_t amount = rate * (world->interval_sim * 1e-6); + + //printf ( "moving %.2f joules from %s to %s\n", + // amount, token, toucher->token ); - //printf( "mod %s test collision done.\n", token ); - return hitmod; + // move some joules from me to him + mypp->TransferTo( hispp, amount ); + hispp->charging = true; + } + } + } void Model::CommitTestedPose() @@ -1665,7 +1692,7 @@ Pose startpose = pose; pose = newpose; // do the move provisionally - we might undo it below - Model* hitmod = TestCollision(); + Model* hitmod = TestCollisionTree(); if( hitmod ) pose = startpose; // move failed - put me back where I started @@ -1683,13 +1710,17 @@ return hitmod; } + void Model::UpdatePose( void ) { if( disabled ) return; - if( velocity.x == 0 && velocity.y == 0 && velocity.a == 0 && velocity.z == 0 ) - return; + if( velocity.IsZero() ) + { + PRINT_WARN1( "model %s has velocity zero but its pose is being updated", token ); + return; + } // TODO - control this properly, and maybe do it faster //if( 0 ) Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -175,14 +175,14 @@ max_range_anon, fiducial_raytrace_match, NULL, - false ); + true ); //range = ray.range; Model* hitmod = ray.mod; -// printf( "ray hit %s and was seeking LOS to %s\n", -// hitmod ? hitmod->Token() : "null", -// him->Token() ); + //printf( "ray hit %s and was seeking LOS to %s\n", + // hitmod ? hitmod->Token() : "null", + // him->Token() ); // if it was him, we can see him if( hitmod == him ) @@ -262,23 +262,23 @@ if ( !showFiducialData ) return; - // draw the FOV - GLUquadric* quadric = gluNewQuadric(); +// // draw the FOV +// GLUquadric* quadric = gluNewQuadric(); - PushColor( 0,0,0,0.2 ); +// PushColor( 0,0,0,0.2 ); - gluQuadricDrawStyle( quadric, GLU_SILHOUETTE ); +// 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 +// 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 ); - PopColor(); +// gluDeleteQuadric( quadric ); +// PopColor(); if( data->len == 0 ) return; Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/model_load.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -41,7 +41,7 @@ power_pack = new PowerPack( this ); power_pack->capacity = - wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity ); + wf->ReadFloat( wf_entity, "joules_capacity", power_pack->capacity ); } /** if the capacity has been specified, limit the store to the capacity */ @@ -53,7 +53,7 @@ power_pack->stored, power_pack->capacity ); } - + // use my own pack or an ancestor's for the other energy properties PowerPack* pp = FindPowerPack(); @@ -65,11 +65,15 @@ if( (watts_give > 0.0) && !pp) PRINT_WARN1( "Model %s: Setting \"watts_give\" has no effect unless \"joules\" is specified for this model or a parent", token ); + if( watts_give ) // need to get the world to test this model for charging + if( ! g_list_find( world->charge_list, this ) ) + world->charge_list = g_list_append( world->charge_list, this ); + watts_take = wf->ReadFloat( wf_entity, "take_watts", watts_take ); if( (watts_take > 0.0) & !pp ) PRINT_WARN1( "Model %s: Setting \"watts_take\" has no effect unless \"joules\" is specified for this model or a parent", token ); - + if( wf->PropertyExists( wf_entity, "debug" ) ) { PRINT_WARN2( "debug property specified for model %d %s\n", Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/model_position.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -475,6 +475,11 @@ Model::Shutdown(); } +void ModelPosition::Stop() +{ + SetSpeed( 0,0,0 ); +} + void ModelPosition::SetSpeed( double x, double y, double a ) { assert( ! isnan(x) ); Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/powerpack.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -27,14 +27,35 @@ const double height = 0.5; const double width = 0.2; + // draw an electric zap +// glPolygonMode( GL_FRONT, GL_LINE ); +// glBegin( GL_POLYGON ); +// glVertex2i( 0, 0 ); +// glVertex2i( 3, 2 ); +// glVertex2i( 1, 2 ); +// glEnd(); + +// glVertex2i( 1, 3 ); +// glVertex2i( 0, 3 ); +// glVertex2i( 1, 5 ); +// glVertex2i( 3, 5 ); +// glVertex2i( 4, 3 ); +// glVertex2i( 5, 3 ); +// glVertex2i( 4, 4 ); +// glVertex2i( 5, 4); +// glEnd(); + //} + double percent = stored/capacity * 100.0; - + + const double alpha = 0.5; + if( percent > 50 ) - glColor4f( 0,1,0, 0.7 ); // green + glColor4f( 0,1,0, alpha ); // green else if( percent > 25 ) - glColor4f( 1,0,1, 0.7 ); // magenta + glColor4f( 1,0,1, alpha ); // magenta else - glColor4f( 1,0,0, 0.7 ); // red + glColor4f( 1,0,0, alpha ); // red static char buf[6]; snprintf( buf, 6, "%.0f", percent ); @@ -59,6 +80,36 @@ glVertex2f( width, fullness ); glEnd(); + if( stored < 0.0 ) // inifinite supply! + { + // draw an arrow toward the top + glBegin( GL_LINES ); + glVertex2f( width/3.0, height/3.0 ); + glVertex2f( 2.0 * width/3, height/3.0 ); + + glVertex2f( width/3.0, height/3.0 ); + glVertex2f( width/3.0, height - height/5.0 ); + + glVertex2f( width/3.0, height - height/5.0 ); + glVertex2f( 0, height - height/5.0 ); + + glVertex2f( 0, height - height/5.0 ); + glVertex2f( width/2.0, height ); + + glVertex2f( width/2.0, height ); + glVertex2f( width, height - height/5.0 ); + + glVertex2f( width, height - height/5.0 ); + glVertex2f( 2.0 * width/3.0, height - height/5.0 ); + + glVertex2f( 2.0 * width/3.0, height - height/5.0 ); + glVertex2f( 2.0 * width/3, height/3.0 ); + + glEnd(); + + } + + if( charging ) { glLineWidth( 6.0 ); @@ -85,8 +136,6 @@ void PowerPack::Add( stg_joules_t j ) { stored += MIN( RemainingCapacity(), j ); - - charging = true; } void PowerPack::Subtract( stg_joules_t j ) @@ -97,6 +146,9 @@ void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount ) { + //printf( "amount %.2f stored %.2f dest capacity %.2f\n", + // amount, stored, dest->RemainingCapacity() ); + // if stored is non-negative we can't transfer more than the stored // amount. If it is negative, we have infinite energy stored if( stored >= 0.0 ) @@ -105,10 +157,10 @@ // we can't transfer more than he can take amount = MIN( amount, dest->RemainingCapacity() ); - printf( "%s receives %.3f J from %s\n", - mod->Token(), amount, dest->mod->Token() ); + + //printf( "%s gives %.3f J to %s\n", + // mod->Token(), amount, dest->mod->Token() ); - Subtract( amount ); dest->Add( amount ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/stage.hh 2009-02-07 02:17:30 UTC (rev 7319) @@ -305,6 +305,8 @@ printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", prefix, x,y,z,a ); } + + bool IsZero(){ return( !(x || y || z || a )); }; }; /** Specify an object's basic geometry: position and rectangular @@ -820,6 +822,7 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids + GList* charge_list; ///< Models which receive charge are listed here bool destroy; bool dirty; ///< iff true, a gui redraw would be required GHashTable* models_by_name; ///< the models that make up the world, indexed by name @@ -933,13 +936,19 @@ bool PastQuitTime(); void StartUpdatingModel( Model* mod ) - { update_list = g_list_append( update_list, mod ); } + { + if( ! g_list_find( update_list, mod ) ) + update_list = g_list_append( update_list, mod ); + } void StopUpdatingModel( Model* mod ) { update_list = g_list_remove( update_list, mod ); } void StartUpdatingModelPose( Model* mod ) - { velocity_list = g_list_append( velocity_list, mod ); } + { + if( ! g_list_find( velocity_list, mod ) ) + velocity_list = g_list_append( velocity_list, mod ); + } void StopUpdatingModelPose( Model* mod ) { velocity_list = g_list_remove( velocity_list, mod ); } @@ -1047,8 +1056,12 @@ // /** Prepare to render the block in a new position in global coordinates */ // void SetPoseTentative( const Pose pose ); + + GList* AppendTouchingModels( GList* list ); + + /** Returns the first model that shares a bitmap cell with this model */ Model* TestCollision(); - + void SwitchToTestedCells(); void Load( Worldfile* wf, int entity ); @@ -1122,9 +1135,13 @@ void CallDisplayList( Model* mod ); void Clear() ; /** deletes all blocks from the group */ + GList* AppendTouchingModels( GList* list ); + //void AddTouchingModelsToList( GList* list ); + /** Returns a pointer to the first model detected to be colliding with a block in this group, or NULL, if none are detected. */ Model* TestCollision(); + void SwitchToTestedCells(); void Map(); @@ -1392,6 +1409,12 @@ /** Transfer some stored energy to another power pack */ void TransferTo( PowerPack* dest, stg_joules_t amount ); + + double ProportionRemaining() + { return( stored / capacity ); } + + void Print( const char* prefix ) + { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); } }; class Visibility @@ -1631,10 +1654,17 @@ void registerOption( Option* opt ) { drawOptions.push_back( opt ); } + GList* AppendTouchingModels( GList* list ); + //void AddTouchingModelsToList( GList* list ); + /** Check to see if the current pose will yield a collision with obstacles. Returns a pointer to the first entity we are in collision with, or NULL if no collision exists. */ Model* TestCollision(); + + /** Recursively call TestCollision() on this model and all its + descendents */ + Model* TestCollisionTree(); void CommitTestedPose(); @@ -1690,6 +1720,7 @@ virtual void Shutdown(); virtual void Update(); virtual void UpdatePose(); + virtual void UpdateCharge(); void StartUpdating(); void StopUpdating(); @@ -1715,7 +1746,7 @@ void PopCoords(); /** Draw the image stored in texture_id above the model */ - void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); + void DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width=1.0, double height=1.0 ); /** static wrapper for DrawBlocks() */ @@ -2575,6 +2606,8 @@ void SetZSpeed( double z ); void SetTurnSpeed( double a ); void SetSpeed( Velocity vel ); + /** Set velocity along all axes to to zero. */ + void Stop(); /** Sets the control mode to STG_POSITION_CONTROL_POSITION and sets the goal pose */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/libstage/world.cc 2009-02-07 02:17:30 UTC (rev 7319) @@ -73,6 +73,7 @@ double ppm ) : // private + charge_list( NULL ), destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), @@ -426,6 +427,10 @@ // upate all positions first LISTMETHOD( velocity_list, Model*, UpdatePose ); + + // test all models that supply charge to see if they are touching + // something that takes charge + LISTMETHOD( charge_list, Model*, UpdateCharge ); // then update all sensors if( worker_threads == 0 ) // do all the work in this thread Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-05 23:53:46 UTC (rev 7318) +++ code/stage/trunk/worlds/fasr.world 2009-02-07 02:17:30 UTC (rev 7319) @@ -7,24 +7,24 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 30 # real-time interval between simulation updates in milliseconds +interval_real 0 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 # threads may speed things up here depending on available CPU cores & workload -threadpool 0 -# threadpool 2 + threadpool 0 +# threadpool 3 # configure the GUI window window ( - size [ 600.000 599.000 ] + size [ 902.000 856.000 ] - center [ 4.605 -2.392 ] + center [ 6.726 -2.592 ] rotate [ 0 0 ] - scale 68.242 + scale 116.966 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -61,80 +61,77 @@ ctrl "sink" ) -define autorob pioneer2dx -( - sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) - ctrl "fasr" - joules 1000000 - fiducial_return 1 -) - define charging_bump model ( - pose [0.210 0 -0.100 0 ] - size [0.050 0.100 0.100] - take_watts 300.0 + pose [0.240 0 -0.100 0 ] + size [0.120 0.050 0.100] + take_watts 1000.0 color "orange" - obstacle_return 1 + obstacle_return 0 ) +define autorob pioneer2dx +( + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + ctrl "fasr" + joules 100000 + joules_capacity 400000 + fiducial_return 0 + charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) +) + define charge_station model ( - color "yellow" - size [ 0.100 0.500 0.400 ] - joules -1 # infinite storage - give_watts 500 -) + size [ 0.100 0.300 0.100 ] + color "purple" -charge_station -( - pose [ 7.941 -3.001 0 0 ] -) + model( color "purple" size [0.100 0.100 0.400] pose [ 0 0.100 0 0 ] ) -autorob -( - pose [2.933 0.917 0 -147.323] - color "magenta" - joules 10000 - joules_capacity 100000 - charging_bump() -) + model( + pose [ 0.010 0 0 0 ] + color "yellow" + size [ 0.050 0.100 0.100 ] + joules -1 # infinite storage + give_watts 1000 + fiducial_return 2 + ) -autorob -( - pose [5.757 -3.528 0 14.941] - color "green" - charging_bump( take_watts 0 give_watts 300.0 ) + model( color "purple" size [0.100 0.100 0.400] pose [ 0 -0.100 0 0 ] ) ) -autorob( pose [5.937 4.858 0 -147.503] ) +charge_station( pose [ 7.803 -1.332 0 34.377 ] ) +charge_station( pose [ 7.940 -2.349 0 0 ] ) +charge_station( pose [ 7.931 -3.367 0 0 ] ) +charge_station( pose [ 7.931 -4.444 0 0 ] ) + +autorob( pose [4.144 6.834 0 -98.076] ) autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.664 5.938 0 107.666] ) -autorob( pose [7.016 6.428 0 -128.279] ) +autorob( pose [5.615 6.185 0 107.666] ) +autorob( pose [7.028 6.502 0 -128.279] ) autorob( pose [5.750 4.137 0 -97.047] ) autorob( pose [4.909 6.097 0 -44.366] ) autorob( pose [6.898 4.775 0 -117.576] ) -autorob( pose [7.012 5.706 0 129.497] ) - -autorob( pose [6.616 6.893 0 170.743] ) +autorob( pose [7.394 5.595 0 129.497] ) +autorob( pose [6.468 6.708 0 170.743] ) autorob( pose [6.451 4.189 0 -61.453] ) -autorob( pose [5.098 6.788 0 -61.295] ) -autorob( pose [4.374 5.163 0 -147.713] ) -autorob( pose [4.999 4.230 0 -125.236] ) -autorob( pose [3.533 4.220 0 78.789] ) + +autorob( pose [5.246 6.813 0 -61.295] ) +autorob( pose [4.127 5.388 0 -147.713] ) +autorob( pose [5.020 4.213 0 -125.236] ) +autorob( pose [3.286 4.715 0 78.789] ) autorob( pose [5.440 5.317 0 -26.545] ) -autorob( pose [7.518 6.973 0 163.239] ) +autorob( pose [7.641 6.998 0 163.239] ) autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [5.940 6.768 0 77.301] ) +autorob( pose [5.471 7.446 0 77.301] ) +autorob( pose [7.122 4.175 0 -31.440] ) +autorob( pose [5.944 6.951 0 2.937] ) -#autorob( pose [7.122 4.175 0 -31.440] ) -#autorob( pose [6.203 6.963 0 2.937] ) #autorob( pose [6.800 5.897 0 -103.060] ) -#autorob( pose [6.331 6.450 0 -103.060] ) +#autorob( pose [6.405 5.291 0 -103.060] ) #autorob( pose [5.974 5.725 0 -103.060] ) #autorob( pose [4.151 7.272 0 53.540] ) #autorob( pose [6.545 7.459 0 2.937] ) -#autorob( pose [7.225 7.459 0 34.450] ) +#autorob( pose [7.237 7.533 0 34.450] ) #autorob( pose [3.875 6.533 0 134.717] ) -#autorob( pose [3.944 5.045 0 -103.060] ) +#autorob( pose [3.944 4.674 0 -103.060] ) #autorob( pose [4.634 6.897 0 -103.060] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-13 07:10:17
|
Revision: 7329 http://playerstage.svn.sourceforge.net/playerstage/?rev=7329&view=rev Author: rtv Date: 2009-02-13 07:10:11 +0000 (Fri, 13 Feb 2009) Log Message: ----------- grippers partially implemented. they don't lift anything yet Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/typetable.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -50,6 +50,7 @@ ModelRanger* ranger; ModelFiducial* fiducial; ModelBlobfinder* blobfinder; + ModelGripper* gripper; Model *source, *sink; int avoidcount, randcount; int work_get, work_put; @@ -82,6 +83,7 @@ ranger( (ModelRanger*)pos->GetUnusedModelOfType( MODEL_TYPE_RANGER )), fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( MODEL_TYPE_FIDUCIAL )), blobfinder( (ModelBlobfinder*)pos->GetUnusedModelOfType( MODEL_TYPE_BLOBFINDER )), + gripper( (ModelGripper*)pos->GetUnusedModelOfType( MODEL_TYPE_GRIPPER )), source(source), sink(sink), avoidcount(0), @@ -133,6 +135,14 @@ void Robot::Dock() { + // close the grippers so they can be pushed into the charger + ModelGripper::data_t gripper_data = gripper->GetData(); + + if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) + gripper->CommandClose(); + else if( gripper_data.lift != ModelGripper::LIFT_UP ) + gripper->CommandUp(); + if( charger_ahoy ) { double a_goal = normalize( charger_bearing ); @@ -183,9 +193,30 @@ void Robot::UnDock() { - if( charger_range < 0.3 ) - pos->SetXSpeed( -0.05 ); + const stg_meters_t gripper_distance = 0.2; + const stg_meters_t back_off_distance = 0.3; + const stg_meters_t back_off_speed = -0.05; + + // back up a bit + if( charger_range < back_off_distance ) + pos->SetXSpeed( back_off_speed ); else + pos->SetXSpeed( 0.0 ); + + // once we have backed off a bit, open and lower the gripper + ModelGripper::data_t gripper_data = gripper->GetData(); + if( charger_range > gripper_distance ) + { + if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) + gripper->CommandOpen(); + else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) + gripper->CommandDown(); + } + + // if the gripper is down and open and we're away from the charger, undock is finished + if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && + gripper_data.lift == ModelGripper::LIFT_DOWN && + charger_range > back_off_distance ) mode = MODE_WORK; } Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-13 07:10:11 UTC (rev 7329) @@ -4,6 +4,7 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs ancestor.cc + model_gripper.cc block.cc blockgroup.cc camera.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/block.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -61,6 +61,80 @@ g_ptr_array_free( candidate_cells, TRUE ); } +void Block::Translate( double x, double y ) +{ + for( unsigned int p=0; p<pt_count; p++) + { + pts[p].x += x; + pts[p].y += y; + } + + // force redraw + mod->blockgroup.BuildDisplayList( mod ); +} + + +double Block::CenterY() +{ + double min = billion; + double max = -billion; + + for( unsigned int p=0; p<pt_count; p++) + { + if( pts[p].y > max ) max = pts[p].y; + if( pts[p].y < min ) min = pts[p].y; + } + + // return the value half way between max and min + return( min + (max - min)/2.0 ); +} + +double Block::CenterX() +{ + double min = billion; + double max = -billion; + + for( unsigned int p=0; p<pt_count; p++) + { + if( pts[p].x > max ) max = pts[p].x; + if( pts[p].x < min ) min = pts[p].x; + } + + // return the value half way between maxx and min + return( min + (max - min)/2.0 ); +} + +void Block::SetCenter( double x, double y ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( x-CenterX(), y-CenterY() ); +} + +void Block::SetCenterY( double y ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( 0, y-CenterY() ); +} + +void Block::SetCenterX( double x ) +{ + // move the block by the distance required to bring its center to + // the requested position + Translate( x-CenterX(), 0 ); +} + +void Block::SetZ( double min, double max ) +{ + local_z.min = min; + local_z.max = max; + + // force redraw + mod->blockgroup.BuildDisplayList( mod ); +} + + stg_color_t Block::GetColor() { return( inherit_color ? mod->color : color ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -374,11 +374,11 @@ } -void Model::AddBlockRect( stg_meters_t x, - stg_meters_t y, - stg_meters_t dx, - stg_meters_t dy, - stg_meters_t dz ) +Block* Model::AddBlockRect( stg_meters_t x, + stg_meters_t y, + stg_meters_t dx, + stg_meters_t dy, + stg_meters_t dz ) { UnMap(); @@ -392,11 +392,15 @@ pts[3].x = x; pts[3].y = y + dy; - blockgroup.AppendBlock( new Block( this, - pts, 4, - 0, dz, - color, - true ) ); + Block* newblock = new Block( this, + pts, 4, + 0, dz, + color, + true ); + + blockgroup.AppendBlock( newblock ); + + return newblock; } Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -262,7 +262,7 @@ if ( !showFiducialData ) return; -// // draw the FOV + // draw the FOV // GLUquadric* quadric = gluNewQuadric(); // PushColor( 0,0,0,0.2 ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/model_laser.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -127,7 +127,6 @@ void ModelLaser::Load( void ) { - Model::Load(); sample_count = wf->ReadInt( wf_entity, "samples", sample_count ); range_min = wf->ReadLength( wf_entity, "range_min", range_min); range_max = wf->ReadLength( wf_entity, "range_max", range_max ); @@ -142,6 +141,8 @@ PRINT_WARN( "laser resolution set < 1. Forcing to 1" ); resolution = 1; } + + Model::Load(); } stg_laser_cfg_t ModelLaser::GetConfig() Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/stage.hh 2009-02-13 07:10:11 UTC (rev 7329) @@ -95,6 +95,7 @@ MODEL_TYPE_BLOBFINDER, MODEL_TYPE_BLINKENLIGHT, MODEL_TYPE_CAMERA, + MODEL_TYPE_GRIPPER, MODEL_TYPE_COUNT // must be the last entry, to count the number of // types } stg_model_type_t; @@ -1085,6 +1086,23 @@ void DrawSolid(); // draw the block in OpenGL as a solid single color void DrawFootPrint(); // draw the projection of the block onto the z=0 plane + /** Translate all points in the block by the indicated amounts */ + void Translate( double x, double y ); + + /** Return the center of the block on the X axis */ + double CenterX(); + /** Return the center of the block on the Y axis */ + double CenterY(); + + /** Set the center of the block on the X axis */ + void SetCenterX( double y ); + /** Set the center of the block on the Y axis */ + void SetCenterY( double y ); + /** Set the center of the block */ + void SetCenter( double x, double y); + + void SetZ( double min, double max ); + void RecordRendering( Cell* cell ) { g_ptr_array_add( rendered_cells, (gpointer)cell ); }; @@ -1154,11 +1172,12 @@ class BlockGroup { friend class Model; + friend class Block; private: int displaylist; + void BuildDisplayList( Model* mod ); - GList* blocks; uint32_t count; Size size; @@ -1921,9 +1940,9 @@ /** Add a block to this model centered at [x,y] with extent [dx, dy, dz] */ - void AddBlockRect( stg_meters_t x, stg_meters_t y, - stg_meters_t dx, stg_meters_t dy, - stg_meters_t dz ); + Block* AddBlockRect( stg_meters_t x, stg_meters_t y, + stg_meters_t dx, stg_meters_t dy, + stg_meters_t dz ); /** remove all blocks from this model, freeing their memory */ void ClearBlocks(); @@ -2277,88 +2296,132 @@ // Set the user-tweakable configuration of the laser void SetConfig( stg_laser_cfg_t cfg ); }; - + // \todo GRIPPER MODEL -------------------------------------------------------- + + + class ModelGripper : public Model + { + public: - // typedef enum { - // STG_GRIPPER_PADDLE_OPEN = 0, // default state - // STG_GRIPPER_PADDLE_CLOSED, - // STG_GRIPPER_PADDLE_OPENING, - // STG_GRIPPER_PADDLE_CLOSING, - // } stg_gripper_paddle_state_t; + enum paddle_state_t { + PADDLE_OPEN = 0, // default state + PADDLE_CLOSED, + PADDLE_OPENING, + PADDLE_CLOSING, + }; + + enum lift_state_t { + LIFT_DOWN = 0, // default state + LIFT_UP, + LIFT_UPPING, // verbed these to match the paddle state + LIFT_DOWNING, + }; + + enum cmd_t { + CMD_NOOP = 0, // default state + CMD_OPEN, + CMD_CLOSE, + CMD_UP, + CMD_DOWN + }; + + /** gripper configuration + */ + struct config_t + { + Size paddle_size; ///< paddle dimensions + + paddle_state_t paddles; + lift_state_t lift; + + double paddle_position; ///< 0.0 = full open, 1.0 full closed + double lift_position; ///< 0.0 = full down, 1.0 full up + + stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle + stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle + bool paddles_stalled; // true iff some solid object stopped + // the paddles closing or opening + + GSList *grip_stack; ///< stack of items gripped + int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited + + double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. + bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically + }; + + + /** gripper data packet + */ + struct data_t + { + paddle_state_t paddles; + lift_state_t lift; - // typedef enum { - // STG_GRIPPER_LIFT_DOWN = 0, // default state - // STG_GRIPPER_LIFT_UP, - // STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state - // STG_GRIPPER_LIFT_DOWNING, - // } stg_gripper_lift_state_t; + double paddle_position; ///< 0.0 = full open, 1.0 full closed + double lift_position; ///< 0.0 = full down, 1.0 full up - // typedef enum { - // STG_GRIPPER_CMD_NOP = 0, // default state - // STG_GRIPPER_CMD_OPEN, - // STG_GRIPPER_CMD_CLOSE, - // STG_GRIPPER_CMD_UP, - // STG_GRIPPER_CMD_DOWN - // } stg_gripper_cmd_type_t; + stg_bool_t inner_break_beam; ///< non-zero iff beam is broken + stg_bool_t outer_break_beam; ///< non-zero iff beam is broken - // /** gripper configuration packet - // */ - // typedef struct - // { - // Size paddle_size; ///< paddle dimensions + stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something - // stg_gripper_paddle_state_t paddles; - // stg_gripper_lift_state_t lift; + stg_bool_t paddles_stalled; // true iff some solid object stopped + // the paddles closing or opening - // double paddle_position; ///< 0.0 = full open, 1.0 full closed - // double lift_position; ///< 0.0 = full down, 1.0 full up + int stack_count; ///< number of objects in stack - // stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle - // stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle - // stg_bool_t paddles_stalled; // true iff some solid object stopped - // // the paddles closing or opening - // GSList *grip_stack; ///< stack of items gripped - // int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited + }; - // double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. + private: + virtual void Update(); + virtual void DataVisualize( Camera* cam ); + + void FixBlocks(); + void PositionPaddles(); - // } stg_gripper_config_t; + config_t cfg; + cmd_t cmd; + + Block* paddle_left; + Block* paddle_right; - // /** gripper command packet - // */ - // typedef struct - // { - // stg_gripper_cmd_type_t cmd; - // int arg; - // } stg_gripper_cmd_t; + public: + static const char* typestr; + static const Size size; + // constructor + ModelGripper( World* world, + Model* parent ); + // destructor + virtual ~ModelGripper(); + + virtual void Load(); + virtual void Save(); - // /** gripper data packet - // */ - // typedef struct - // { - // stg_gripper_paddle_state_t paddles; - // stg_gripper_lift_state_t lift; + void SetConfig( config_t & newcfg ); - // double paddle_position; ///< 0.0 = full open, 1.0 full closed - // double lift_position; ///< 0.0 = full down, 1.0 full up + /** Returns the static state of the gripper */ + config_t GetConfig(){ return cfg; }; - // stg_bool_t inner_break_beam; ///< non-zero iff beam is broken - // stg_bool_t outer_break_beam; ///< non-zero iff beam is broken + /** Returns the dynamic state of the gripper */ + data_t GetData(); + + /** Set the current activity of the gripper. */ + void SetCommand( cmd_t cmd ) { this->cmd = cmd; } - // stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something + /** Command the gripper paddles to close. Wrapper for SetCommand( CMD_CLOSE ). */ + void CommandClose() { SetCommand( CMD_CLOSE ); } + /** Command the gripper paddles to open. Wrapper for SetCommand( CMD_OPEN ). */ + void CommandOpen() { SetCommand( CMD_OPEN ); } + /** Command the gripper lift to go up. Wrapper for SetCommand( CMD_UP ). */ + void CommandUp() { SetCommand( CMD_UP ); } + /** Command the gripper lift to go down. Wrapper for SetCommand( CMD_DOWN ). */ + void CommandDown() { SetCommand( CMD_DOWN ); } + }; - // stg_bool_t paddles_stalled; // true iff some solid object stopped - // // the paddles closing or opening - // int stack_count; ///< number of objects in stack - - - // } stg_gripper_data_t; - - // \todo BUMPER MODEL -------------------------------------------------------- // typedef struct Modified: code/stage/trunk/libstage/typetable.cc =================================================================== --- code/stage/trunk/libstage/typetable.cc 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/libstage/typetable.cc 2009-02-13 07:10:11 UTC (rev 7329) @@ -28,7 +28,10 @@ static Model* CreateModelBlobfinder( World* world, Model* parent ) { return new ModelBlobfinder( world, parent ); } +static Model* CreateModelGripper( World* world, Model* parent ) +{ return new ModelGripper( world, parent ); } + void Stg::RegisterModels() { RegisterModel( MODEL_TYPE_PLAIN, "model", CreateModel ); @@ -39,6 +42,7 @@ RegisterModel( MODEL_TYPE_POSITION, "position", CreateModelPosition ); RegisterModel( MODEL_TYPE_BLOBFINDER, "blobfinder", CreateModelBlobfinder ); RegisterModel( MODEL_TYPE_BLINKENLIGHT, "blinkenlight", CreateModelBlinkenlight); + RegisterModel( MODEL_TYPE_GRIPPER, "gripper", CreateModelGripper); #if DEBUG // human-readable view of the table puts( "Stg::Typetable" ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-10 19:48:30 UTC (rev 7328) +++ code/stage/trunk/worlds/fasr.world 2009-02-13 07:10:11 UTC (rev 7329) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 20 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 @@ -22,9 +22,9 @@ ( size [ 902.000 856.000 ] - center [ 6.726 -2.592 ] - rotate [ 0 0 ] - scale 116.966 + center [ 5.355 5.319 ] + rotate [ 30.000 26.000 ] + scale 126.761 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -77,26 +77,36 @@ joules 100000 joules_capacity 400000 fiducial_return 0 - charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) + + gripper( pose [0.25 0 -0.22 0] + take_watts 1000.0 + fiducial( range 3 ) + # paddles [ "closed" "up" ] + obstacle_return 0 # cheating for simplicity + autosnatch 0 + ) ) define charge_station model ( size [ 0.100 0.300 0.100 ] - color "purple" + color "purple" + + # side blocks to restrict view angle + model( color "purple" size [0.100 0.050 0.400] pose [ 0 0.100 0 0 ] ) + model( color "purple" size [0.100 0.050 0.400] pose [ 0 -0.100 0 0 ] ) - model( color "purple" size [0.100 0.100 0.400] pose [ 0 0.100 0 0 ] ) + # the charging block + model( + pose [ 0.010 0 0 0 ] + color "yellow" + size [ 0.050 0.200 0.150 ] + joules -1 # provides infinite energy + give_watts 1000 + fiducial_return 2 # look for this in the fiducial sensor + ) - model( - pose [ 0.010 0 0 0 ] - color "yellow" - size [ 0.050 0.100 0.100 ] - joules -1 # infinite storage - give_watts 1000 - fiducial_return 2 - ) - - model( color "purple" size [0.100 0.100 0.400] pose [ 0 -0.100 0 0 ] ) ) charge_station( pose [ 7.803 -1.332 0 34.377 ] ) @@ -104,28 +114,29 @@ charge_station( pose [ 7.931 -3.367 0 0 ] ) charge_station( pose [ 7.931 -4.444 0 0 ] ) -autorob( pose [4.144 6.834 0 -98.076] ) -autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.615 6.185 0 107.666] ) -autorob( pose [7.028 6.502 0 -128.279] ) -autorob( pose [5.750 4.137 0 -97.047] ) -autorob( pose [4.909 6.097 0 -44.366] ) -autorob( pose [6.898 4.775 0 -117.576] ) -autorob( pose [7.394 5.595 0 129.497] ) -autorob( pose [6.468 6.708 0 170.743] ) -autorob( pose [6.451 4.189 0 -61.453] ) +autorob( pose [4.144 6.834 0 -98.076] joules 300000 ) +autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) +autorob( pose [5.615 6.185 0 107.666] joules 200000 ) +autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) +autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) +autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) +autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) +autorob( pose [7.394 5.595 0 129.497] joules 400000 ) +autorob( pose [6.468 6.708 0 170.743] joules 100000 ) +autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) -autorob( pose [5.246 6.813 0 -61.295] ) -autorob( pose [4.127 5.388 0 -147.713] ) -autorob( pose [5.020 4.213 0 -125.236] ) -autorob( pose [3.286 4.715 0 78.789] ) -autorob( pose [5.440 5.317 0 -26.545] ) -autorob( pose [7.641 6.998 0 163.239] ) -autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [5.471 7.446 0 77.301] ) -autorob( pose [7.122 4.175 0 -31.440] ) -autorob( pose [5.944 6.951 0 2.937] ) +autorob( pose [5.246 6.813 0 -61.295] joules 300000 ) +autorob( pose [4.127 5.388 0 -147.713] joules 400000 ) +autorob( pose [5.020 4.213 0 -125.236] joules 100000 ) +autorob( pose [3.286 4.715 0 78.789] joules 200000 ) +autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) +autorob( pose [7.641 6.998 0 163.239] joules 400000 ) +#autorob( pose [7.559 4.764 0 -139.066] ) +#autorob( pose [5.471 7.446 0 77.301] ) +#autorob( pose [7.122 4.175 0 -31.440] ) +#autorob( pose [5.944 6.951 0 2.937] ) + #autorob( pose [6.800 5.897 0 -103.060] ) #autorob( pose [6.405 5.291 0 -103.060] ) #autorob( pose [5.974 5.725 0 -103.060] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-13 09:43:24
|
Revision: 7331 http://playerstage.svn.sourceforge.net/playerstage/?rev=7331&view=rev Author: rtv Date: 2009-02-13 09:43:20 +0000 (Fri, 13 Feb 2009) Log Message: ----------- basic gripper working - needs thorough testing Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 09:43:20 UTC (rev 7331) @@ -59,21 +59,8 @@ double charger_range; double charger_heading; nav_mode_t mode; + bool at_dest; - static int LaserUpdate( ModelLaser* mod, Robot* robot ); - static int PositionUpdate( ModelPosition* mod, Robot* robot ); - static int FiducialUpdate( ModelFiducial* mod, Robot* robot ); - static int BlobFinderUpdate( ModelBlobfinder* mod, Robot* robot ); - - void Dock(); - void Work(); - void UnDock(); - bool ObstacleAvoid(); - - // predicate that indicates if we need to charge - bool Hungry(); - bool Full(); - public: Robot( ModelPosition* pos, Model* source, @@ -94,7 +81,8 @@ charger_bearing(0), charger_range(0), charger_heading(0), - mode(MODE_WORK) + mode(MODE_WORK), + at_dest( false ) { // need at least these models to get any work done // (pos must be good, as we used it in the initialization list) @@ -104,381 +92,421 @@ // PositionUpdate() checks to see if we reached source or sink pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this ); + pos->Subscribe(); // LaserUpdate() controls the robot, by reading from laser and // writing to position laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this ); + laser->Subscribe(); - // trivial demos - - if( fiducial ) // optional - fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); - + fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this ); + fiducial->Subscribe(); + + //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate, this ); + gripper->Subscribe(); + if( blobfinder ) // optional - blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); - } -}; - - - -// Stage calls this when the model starts up -extern "C" int Init( Model* mod ) -{ - Robot* robot = new Robot( (ModelPosition*)mod, - mod->GetWorld()->GetModel( "source" ), - mod->GetWorld()->GetModel( "sink" ) ); - - return 0; //ok + { + blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this ); + blobfinder->Subscribe(); + } } - - -void Robot::Dock() -{ - // close the grippers so they can be pushed into the charger - ModelGripper::data_t gripper_data = gripper->GetData(); + void Dock() + { + // close the grippers so they can be pushed into the charger + ModelGripper::data_t gripper_data = gripper->GetData(); - if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) - gripper->CommandClose(); - else if( gripper_data.lift != ModelGripper::LIFT_UP ) - gripper->CommandUp(); + if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) + gripper->CommandClose(); + else if( gripper_data.lift != ModelGripper::LIFT_UP ) + gripper->CommandUp(); - if( charger_ahoy ) - { - double a_goal = normalize( charger_bearing ); + if( charger_ahoy ) + { + double a_goal = normalize( charger_bearing ); -// if( pos->Stalled() ) -// { -// puts( "stalled. stopping" ); -// pos->Stop(); -// } -// else + // if( pos->Stalled() ) + // { + // puts( "stalled. stopping" ); + // pos->Stop(); + // } + // else - if( charger_range > 0.5 ) - { - if( !ObstacleAvoid() ) - { - pos->SetXSpeed( cruisespeed ); - pos->SetTurnSpeed( a_goal ); - } - } - else - { - pos->SetTurnSpeed( a_goal ); - pos->SetXSpeed( 0.02 ); // creep towards it + if( charger_range > 0.5 ) + { + if( !ObstacleAvoid() ) + { + pos->SetXSpeed( cruisespeed ); + pos->SetTurnSpeed( a_goal ); + } + } + else + { + pos->SetTurnSpeed( a_goal ); + pos->SetXSpeed( 0.02 ); // creep towards it - if( charger_range < 0.08 ) // close enough - pos->Stop(); + if( charger_range < 0.08 ) // close enough + pos->Stop(); - if( pos->Stalled() ) // touching - pos->SetXSpeed( -0.01 ); // back off a bit + if( pos->Stalled() ) // touching + pos->SetXSpeed( -0.01 ); // back off a bit - } - } - else - { - //printf( "docking but can't see a charger\n" ); - pos->Stop(); - mode = MODE_WORK; // should get us back on track eventually - } + } + } + else + { + //printf( "docking but can't see a charger\n" ); + pos->Stop(); + mode = MODE_WORK; // should get us back on track eventually + } - // if the battery is charged, go back to work - if( Full() ) - { - //printf( "fully charged, now back to work\n" ); - mode = MODE_UNDOCK; - } -} + // if the battery is charged, go back to work + if( Full() ) + { + //printf( "fully charged, now back to work\n" ); + mode = MODE_UNDOCK; + } + } -void Robot::UnDock() -{ - const stg_meters_t gripper_distance = 0.2; - const stg_meters_t back_off_distance = 0.3; - const stg_meters_t back_off_speed = -0.05; + void UnDock() + { + const stg_meters_t gripper_distance = 0.2; + const stg_meters_t back_off_distance = 0.3; + const stg_meters_t back_off_speed = -0.05; - // back up a bit - if( charger_range < back_off_distance ) - pos->SetXSpeed( back_off_speed ); - else - pos->SetXSpeed( 0.0 ); + // back up a bit + if( charger_range < back_off_distance ) + pos->SetXSpeed( back_off_speed ); + else + pos->SetXSpeed( 0.0 ); - // once we have backed off a bit, open and lower the gripper - ModelGripper::data_t gripper_data = gripper->GetData(); - if( charger_range > gripper_distance ) - { - if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) - gripper->CommandOpen(); - else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) - gripper->CommandDown(); - } + // once we have backed off a bit, open and lower the gripper + ModelGripper::data_t gripper_data = gripper->GetData(); + if( charger_range > gripper_distance ) + { + if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) + gripper->CommandOpen(); + else if( gripper_data.lift != ModelGripper::LIFT_DOWN ) + gripper->CommandDown(); + } - // if the gripper is down and open and we're away from the charger, undock is finished - if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && - gripper_data.lift == ModelGripper::LIFT_DOWN && - charger_range > back_off_distance ) - mode = MODE_WORK; -} + // if the gripper is down and open and we're away from the charger, undock is finished + if( gripper_data.paddles == ModelGripper::PADDLE_OPEN && + gripper_data.lift == ModelGripper::LIFT_DOWN && + charger_range > back_off_distance ) + mode = MODE_WORK; + } -bool Robot::ObstacleAvoid() -{ - bool obstruction = false; - bool stop = false; + bool ObstacleAvoid() + { + bool obstruction = false; + bool stop = false; - // find the closest distance to the left and right and check if - // there's anything in front - double minleft = 1e6; - double minright = 1e6; + // find the closest distance to the left and right and check if + // there's anything in front + double minleft = 1e6; + double minright = 1e6; - // Get the data - uint32_t sample_count=0; - stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); + // Get the data + uint32_t sample_count=0; + stg_laser_sample_t* scan = laser->GetSamples( &sample_count ); - for (uint32_t i = 0; i < sample_count; i++) - { - if( verbose ) printf( "%.3f ", scan[i].range ); + for (uint32_t i = 0; i < sample_count; i++) + { + if( verbose ) printf( "%.3f ", scan[i].range ); - if( (i > (sample_count/4)) - && (i < (sample_count - (sample_count/4))) - && scan[i].range < minfrontdistance) - { - if( verbose ) puts( " obstruction!" ); - obstruction = true; - } + if( (i > (sample_count/4)) + && (i < (sample_count - (sample_count/4))) + && scan[i].range < minfrontdistance) + { + if( verbose ) puts( " obstruction!" ); + obstruction = true; + } - if( scan[i].range < stopdist ) - { - if( verbose ) puts( " stopping!" ); - stop = true; - } + if( scan[i].range < stopdist ) + { + if( verbose ) puts( " stopping!" ); + stop = true; + } - if( i > sample_count/2 ) - minleft = MIN( minleft, scan[i].range ); - else - minright = MIN( minright, scan[i].range ); - } + if( i > sample_count/2 ) + minleft = MIN( minleft, scan[i].range ); + else + minright = MIN( minright, scan[i].range ); + } - if( verbose ) - { - puts( "" ); - printf( "minleft %.3f \n", minleft ); - printf( "minright %.3f\n ", minright ); - } + if( verbose ) + { + puts( "" ); + printf( "minleft %.3f \n", minleft ); + printf( "minright %.3f\n ", minright ); + } - if( obstruction || stop || (avoidcount>0) ) - { - if( verbose ) printf( "Avoid %d\n", avoidcount ); + if( obstruction || stop || (avoidcount>0) ) + { + if( verbose ) printf( "Avoid %d\n", avoidcount ); - pos->SetXSpeed( stop ? 0.0 : avoidspeed ); + pos->SetXSpeed( stop ? 0.0 : avoidspeed ); - /* once we start avoiding, select a turn direction and stick - with it for a few iterations */ - if( avoidcount < 1 ) - { - if( verbose ) puts( "Avoid START" ); - avoidcount = random() % avoidduration + avoidduration; + /* once we start avoiding, select a turn direction and stick + with it for a few iterations */ + if( avoidcount < 1 ) + { + if( verbose ) puts( "Avoid START" ); + avoidcount = random() % avoidduration + avoidduration; - if( minleft < minright ) - { - pos->SetTurnSpeed( -avoidturn ); - if( verbose ) printf( "turning right %.2f\n", -avoidturn ); - } - else - { - pos->SetTurnSpeed( +avoidturn ); - if( verbose ) printf( "turning left %2f\n", +avoidturn ); - } - } + if( minleft < minright ) + { + pos->SetTurnSpeed( -avoidturn ); + if( verbose ) printf( "turning right %.2f\n", -avoidturn ); + } + else + { + pos->SetTurnSpeed( +avoidturn ); + if( verbose ) printf( "turning left %2f\n", +avoidturn ); + } + } - avoidcount--; + avoidcount--; - return true; // busy avoding obstacles - } + return true; // busy avoding obstacles + } - return false; // didn't have to avoid anything -} + return false; // didn't have to avoid anything + } -void Robot::Work() -{ - if( ! ObstacleAvoid() ) - { - if( verbose ) puts( "Cruise" ); + void Work() + { + if( ! ObstacleAvoid() ) + { + if( verbose ) puts( "Cruise" ); - //avoidcount = 0; - pos->SetXSpeed( cruisespeed ); + ModelGripper::data_t gdata = gripper->GetData(); + + //avoidcount = 0; + pos->SetXSpeed( cruisespeed ); - Pose pose = pos->GetPose(); + Pose pose = pos->GetPose(); - int x = (pose.x + 8) / 4; - int y = (pose.y + 8) / 4; + int x = (pose.x + 8) / 4; + int y = (pose.y + 8) / 4; - // oh what an awful bug - 5 hours to track this down. When using - // this controller in a world larger than 8*8 meters, a_goal can - // sometimes be NAN. Causing trouble WAY upstream. - if( x > 3 ) x = 3; - if( y > 3 ) y = 3; - if( x < 0 ) x = 0; - if( y < 0 ) y = 0; + // oh what an awful bug - 5 hours to track this down. When using + // this controller in a world larger than 8*8 meters, a_goal can + // sometimes be NAN. Causing trouble WAY upstream. + if( x > 3 ) x = 3; + if( y > 3 ) y = 3; + if( x < 0 ) x = 0; + if( y < 0 ) y = 0; - double a_goal = - dtor( pos->GetFlagCount() ? have[y][x] : need[y][x] ); + double a_goal = + dtor( ( pos->GetFlagCount() || gdata.stack_count ) ? have[y][x] : need[y][x] ); - // if we are low on juice - find the direction to the recharger instead - if( Hungry() ) - { - //puts( "hungry - using refuel map" ); + // if we are low on juice - find the direction to the recharger instead + if( Hungry() ) + { + //puts( "hungry - using refuel map" ); - // use the refuel map - a_goal = dtor( refuel[y][x] ); + // use the refuel map + a_goal = dtor( refuel[y][x] ); - if( charger_ahoy ) // I see a charger while hungry! - mode = MODE_DOCK; - } + if( charger_ahoy ) // I see a charger while hungry! + mode = MODE_DOCK; + } + else + { + if( ! at_dest ) + { + if( gdata.beam[0] ) // inner break beam broken + gripper->CommandClose(); + } + } + + assert( ! isnan(a_goal ) ); + assert( ! isnan(pose.a ) ); + + double a_error = normalize( a_goal - pose.a ); - assert( ! isnan(a_goal ) ); - assert( ! isnan(pose.a ) ); + assert( ! isnan(a_error) ); - double a_error = normalize( a_goal - pose.a ); - - assert( ! isnan(a_error) ); - - pos->SetTurnSpeed( a_error ); - } -} + pos->SetTurnSpeed( a_error ); + } + } -// inspect the laser data and decide what to do -int Robot::LaserUpdate( ModelLaser* laser, Robot* robot ) -{ -// if( laser->power_pack && laser->power_pack->charging ) -// printf( "model %s power pack @%p is charging\n", -// laser->Token(), laser->power_pack ); + // inspect the laser data and decide what to do + static int LaserUpdate( ModelLaser* laser, Robot* robot ) + { + // if( laser->power_pack && laser->power_pack->charging ) + // printf( "model %s power pack @%p is charging\n", + // laser->Token(), laser->power_pack ); - if( laser->GetSamples(NULL) == NULL ) - return 0; + if( laser->GetSamples(NULL) == NULL ) + return 0; - switch( robot->mode ) - { - case MODE_DOCK: - //puts( "DOCK" ); - robot->Dock(); - break; + switch( robot->mode ) + { + case MODE_DOCK: + //puts( "DOCK" ); + robot->Dock(); + break; - case MODE_WORK: - //puts( "WORK" ); - robot->Work(); - break; + case MODE_WORK: + //puts( "WORK" ); + robot->Work(); + break; - case MODE_UNDOCK: - //puts( "UNDOCK" ); - robot->UnDock(); - break; + case MODE_UNDOCK: + //puts( "UNDOCK" ); + robot->UnDock(); + break; - default: - printf( "unrecognized mode %u\n", robot->mode ); - } + default: + printf( "unrecognized mode %u\n", robot->mode ); + } - //if( robot->charger_ahoy ) - //return 1; - //else + //if( robot->charger_ahoy ) + //return 1; + //else return 0; -} + } -bool Robot::Hungry() -{ - return( pos->FindPowerPack()->ProportionRemaining() < 0.25 ); -} + bool Hungry() + { + return( pos->FindPowerPack()->ProportionRemaining() < 0.25 ); + } -bool Robot::Full() -{ - return( pos->FindPowerPack()->ProportionRemaining() > 0.95 ); -} + bool Full() + { + return( pos->FindPowerPack()->ProportionRemaining() > 0.95 ); + } -int Robot::PositionUpdate( ModelPosition* pos, Robot* robot ) -{ - Pose pose = pos->GetPose(); + static int PositionUpdate( ModelPosition* pos, Robot* robot ) + { + Pose pose = pos->GetPose(); - //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", - // pose.x, pose.y, pose.z, pose.a ); + //printf( "Pose: [%.2f %.2f %.2f %.2f]\n", + // pose.x, pose.y, pose.z, pose.a ); - //pose.z += 0.0001; - //robot->pos->SetPose( pose ); + //pose.z += 0.0001; + //robot->pos->SetPose( pose ); - if( pos->GetFlagCount() < payload && - hypot( -7-pose.x, -7-pose.y ) < 2.0 ) - { - if( ++robot->work_get > workduration ) - { - // protect source from concurrent access - robot->source->Lock(); + if( pos->GetFlagCount() < payload && + hypot( -7-pose.x, -7-pose.y ) < 2.0 ) + { + if( ++robot->work_get > workduration ) + { + // protect source from concurrent access + robot->source->Lock(); - // transfer a chunk from source to robot - pos->PushFlag( robot->source->PopFlag() ); - robot->source->Unlock(); + // transfer a chunk from source to robot + pos->PushFlag( robot->source->PopFlag() ); + robot->source->Unlock(); - robot->work_get = 0; - } - } + robot->work_get = 0; + } + } - if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) - { - if( ++robot->work_put > workduration ) - { - // protect sink from concurrent access - robot->sink->Lock(); + robot->at_dest = false; - //puts( "dropping" ); - // transfer a chunk between robot and goal - robot->sink->PushFlag( pos->PopFlag() ); - robot->sink->Unlock(); + if( hypot( 7-pose.x, 7-pose.y ) < 1.0 ) + { + robot->at_dest = true; - robot->work_put = 0; - } - } + if( ++robot->work_put > workduration ) + { + // protect sink from concurrent access + robot->sink->Lock(); + + //puts( "dropping" ); + // transfer a chunk between robot and goal + robot->sink->PushFlag( pos->PopFlag() ); + robot->sink->Unlock(); + + robot->work_put = 0; + + robot->gripper->CommandOpen(); + } + } - return 0; // run again -} + return 0; // run again + } -int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot ) -{ - robot->charger_ahoy = false; + 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]; + for( unsigned int i = 0; i < mod->fiducial_count; i++ ) + { + stg_fiducial_t* f = &mod->fiducials[i]; - //printf( "fiducial %d is %d at %.2f m %.2f radians\n", - // i, f->id, f->range, f->bearing ); + //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; + 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; - } - } + //printf( "charger at %.2f radians\n", robot->charger_bearing ); + break; + } + } - return 0; // run again -} + return 0; // run again + } -int Robot::BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot ) + static int BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot ) + { + unsigned int blob_count = 0; + stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count ); + + if( blobs && (blob_count>0) ) + { + printf( "%s sees %u blobs\n", blobmod->Token(), blob_count ); + } + + return 0; + } + + static int GripperUpdate( ModelGripper* grip, Robot* robot ) + { + ModelGripper::data_t gdata = grip->GetData(); + + printf( "BREAKBEAMS %s %s\n", + gdata.beam[0] ? gdata.beam[0]->Token() : "<null>", + gdata.beam[1] ? gdata.beam[1]->Token() : "<null>" ); + + printf( "CONTACTS %s %s\n", + gdata.contact[0] ? gdata.contact[0]->Token() : "<null>", + gdata.contact[1] ? gdata.contact[1]->Token() : "<null>"); + + + return 0; + } + + +}; + + +// Stage calls this when the model starts up +extern "C" int Init( Model* mod ) { - unsigned int blob_count = 0; - stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count ); + Robot* robot = new Robot( (ModelPosition*)mod, + mod->GetWorld()->GetModel( "source" ), + mod->GetWorld()->GetModel( "sink" ) ); + + return 0; //ok +} - if( blobs && (blob_count>0) ) - { - printf( "%s sees %u blobs\n", blobmod->Token(), blob_count ); - } - return 0; -} + Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 09:43:20 UTC (rev 7331) @@ -23,12 +23,6 @@ // and replace the list in the hash table g_hash_table_insert( callbacks, key, cb_list ); - - // if the callback was an update function, add this model to the - // world's list of models that need updating (if it's not there - // already) - //if( address == (void*)&update ) - //stg_world_start_updating_model( world, this ); } @@ -76,9 +70,7 @@ // return the number of callbacks now in the list. Useful for // detecting when the list is empty. - //return g_list_length( cb_list ); - - return 0; + return g_list_length( cb_list ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/libstage/stage.hh 2009-02-13 09:43:20 UTC (rev 7331) @@ -2091,13 +2091,13 @@ void AddUpdateCallback( stg_model_callback_t cb, void* user ) { AddCallback( &hooks.update, cb, user ); - Subscribe(); // if attaching a callback here, assume we want updates to happen + //Subscribe(); // if attaching a callback here, assume we want updates to happen } void RemoveUpdateCallback( stg_model_callback_t cb ) { RemoveCallback( &hooks.update, cb ); - Unsubscribe(); + //Unsubscribe(); } /** named-property interface @@ -2326,6 +2326,7 @@ CMD_DOWN }; + /** gripper configuration */ struct config_t @@ -2338,16 +2339,19 @@ double paddle_position; ///< 0.0 = full open, 1.0 full closed double lift_position; ///< 0.0 = full down, 1.0 full up - stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle - stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle bool paddles_stalled; // true iff some solid object stopped // the paddles closing or opening - GSList *grip_stack; ///< stack of items gripped + GList*grip_stack; ///< stack of items gripped int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically + + double break_beam_inset[2]; ///< distance from the end of the paddle + + Model* beam[2]; ///< points to a model detected by the beams + Model* contact[2]; ///< pointers to a model detected by the contacts }; @@ -2361,17 +2365,14 @@ double paddle_position; ///< 0.0 = full open, 1.0 full closed double lift_position; ///< 0.0 = full down, 1.0 full up - stg_bool_t inner_break_beam; ///< non-zero iff beam is broken - stg_bool_t outer_break_beam; ///< non-zero iff beam is broken - stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something - stg_bool_t paddles_stalled; // true iff some solid object stopped // the paddles closing or opening + Model* beam[2]; ///< points to a model detected by the beams + Model* contact[2]; ///< pointers to a model detected by the contacts + int stack_count; ///< number of objects in stack - - }; private: @@ -2380,6 +2381,8 @@ void FixBlocks(); void PositionPaddles(); + void UpdateBreakBeams(); + void UpdateContacts(); config_t cfg; cmd_t cmd; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-13 07:12:04 UTC (rev 7330) +++ code/stage/trunk/worlds/fasr.world 2009-02-13 09:43:20 UTC (rev 7331) @@ -22,9 +22,9 @@ ( size [ 902.000 856.000 ] - center [ 5.355 5.319 ] - rotate [ 30.000 26.000 ] - scale 126.761 + center [ 3.982 5.051 ] + rotate [ 0 0 ] + scale 168.655 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -79,12 +79,12 @@ fiducial_return 0 # charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) ) - gripper( pose [0.25 0 -0.22 0] + gripper( pose [0.250 0 -0.220 0] take_watts 1000.0 fiducial( range 3 ) # paddles [ "closed" "up" ] obstacle_return 0 # cheating for simplicity - autosnatch 0 + # autosnatch 1 ) ) @@ -94,14 +94,14 @@ color "purple" # side blocks to restrict view angle - model( color "purple" size [0.100 0.050 0.400] pose [ 0 0.100 0 0 ] ) - model( color "purple" size [0.100 0.050 0.400] pose [ 0 -0.100 0 0 ] ) + model( color "purple" size [0.100 0.050 0.250] pose [ 0 0.100 0 0 ] ) + model( color "purple" size [0.100 0.050 0.250] pose [ 0 -0.100 0 0 ] ) # the charging block model( pose [ 0.010 0 0 0 ] color "yellow" - size [ 0.050 0.200 0.150 ] + size [ 0.050 0.200 0.100 ] joules -1 # provides infinite energy give_watts 1000 fiducial_return 2 # look for this in the fiducial sensor @@ -114,7 +114,46 @@ charge_station( pose [ 7.931 -3.367 0 0 ] ) charge_station( pose [ 7.931 -4.444 0 0 ] ) -autorob( pose [4.144 6.834 0 -98.076] joules 300000 ) +define puck model ( + size [0.120 0.120 0.100] + color "violet" + gripper_return 1 + obstacle_return 0 +) + +puck( pose [ 4.519 6.829 0 0 ] ) +puck( pose [ 4.088 7.018 0 0 ] ) +puck( pose [ 4.409 7.207 0 0 ] ) +puck( pose [ 3.589 7.324 0 0 ] ) +puck( pose [ 3.729 6.893 0 0 ] ) +puck( pose [ 3.714 7.683 0 0 ] ) +puck( pose [ 3.869 7.275 0 0 ] ) +puck( pose [ 4.039 6.719 0 0 ] ) +puck( pose [ 4.269 6.704 0 0 ] ) +puck( pose [ 3.752 7.101 0 0 ] ) +puck( pose [ 4.243 7.033 0 0 ] ) +puck( pose [ 4.666 7.052 0 0 ] ) +puck( pose [ 3.820 6.629 0 0 ] ) + +puck( pose [ 3.930 6.381 0 0 ] ) +puck( pose [ 4.560 6.269 0 0 ] ) +puck( pose [ 4.391 5.814 0 0 ] ) +puck( pose [ 3.475 5.926 0 0 ] ) +puck( pose [ 3.205 6.595 0 0 ] ) +puck( pose [ 3.549 6.393 0 0 ] ) +puck( pose [ 3.957 5.640 0 0 ] ) +puck( pose [ 3.825 5.781 0 0 ] ) +puck( pose [ 4.789 5.122 0 0 ] ) +puck( pose [ 4.167 5.850 0 0 ] ) +puck( pose [ 3.652 6.101 0 0 ] ) +puck( pose [ 3.957 5.966 0 0 ] ) +puck( pose [ 4.685 5.982 0 0 ] ) +puck( pose [ 4.274 6.152 0 0 ] ) +puck( pose [ 4.380 6.491 0 0 ] ) +puck( pose [ 4.870 6.449 0 0 ] ) +puck( pose [ 3.588 6.635 0 0 ] ) + +autorob( pose [5.462 7.216 0 -163.478] joules 300000 ) autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) autorob( pose [5.615 6.185 0 107.666] joules 200000 ) autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-13 18:56:02
|
Revision: 7333 http://playerstage.svn.sourceforge.net/playerstage/?rev=7333&view=rev Author: rtv Date: 2009-02-13 18:55:58 +0000 (Fri, 13 Feb 2009) Log Message: ----------- added missing file Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc Added Paths: ----------- code/stage/trunk/libstage/model_gripper.cc Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 09:46:46 UTC (rev 7332) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 18:55:58 UTC (rev 7333) @@ -416,6 +416,8 @@ { robot->at_dest = true; + robot->gripper->CommandOpen(); + if( ++robot->work_put > workduration ) { // protect sink from concurrent access @@ -428,7 +430,6 @@ robot->work_put = 0; - robot->gripper->CommandOpen(); } } Added: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc (rev 0) +++ code/stage/trunk/libstage/model_gripper.cc 2009-02-13 18:55:58 UTC (rev 7333) @@ -0,0 +1,688 @@ +/////////////////////////////////////////////////////////////////////////// +// +// File: model_gripper.cc +// Authors: Richard Vaughan <va...@sf...> +// Doug Blank +// Date: 21 April 2005 +// +// CVS info: +// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/src/model_gripper.c,v $ +// $Author: thjc $ +// $Revision: 1.26.8.1 $ +// +/////////////////////////////////////////////////////////////////////////// + +/** +@ingroup model +@defgroup model_gripper Gripper model + +The ranger model simulates a simple two-fingered gripper with two +internal break-beams, similar to the the Pioneer gripper. + +<h2>Worldfile properties</h2> + +@par Summary and default values + +@verbatim +gripper +( + # gripper properties + <none> + + # model properties + size [0.12 0.28] +) +@endverbatim + +@par Notes + +@par Details + +*/ + + +#include <sys/time.h> +#include <math.h> +#include "stage.hh" +#include "worldfile.hh" +using namespace Stg; + +// TODO - simulate energy use when moving grippers + +ModelGripper::ModelGripper( World* world, + Model* parent ) + : Model( world, parent, MODEL_TYPE_GRIPPER ), + cfg(), // configured below + cmd( CMD_NOOP ) +{ + // set up a gripper-specific config structure + cfg.paddle_size.x = 0.66; // proportion of body length that is paddles + cfg.paddle_size.y = 0.1; // proportion of body width that is paddles + cfg.paddle_size.z = 0.4; // proportion of body height that is paddles + + cfg.paddles = PADDLE_OPEN; + cfg.lift = LIFT_DOWN; + cfg.paddle_position = 0.0; + cfg.lift_position = 0.0; + cfg.paddles_stalled = false; + cfg.autosnatch = false; + cfg.grip_stack = NULL; + cfg.grip_stack_size = 1; + + // place the break beam sensors at 1/4 and 3/4 the length of the paddle + cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x; + cfg.break_beam_inset[1] = 1.0/4.0 * cfg.paddle_size.x; + + cfg.close_limit = 1.0; + + SetColor( stg_color_pack( 0.3, 0.3, 0.3, 0 )); + + FixBlocks(); + + // default size + Geom geom; + geom.pose.x = 0.0; + geom.pose.y = 0.0; + geom.pose.a = 0.0; + geom.size.x = 0.2; + geom.size.y = 0.3; + geom.size.z = 0.2; + SetGeom( geom ); + + //Startup(); + + PositionPaddles(); +} + +ModelGripper::~ModelGripper() +{ + /* do nothing */ +} + + +void ModelGripper::Load() +{ + cfg.autosnatch = wf->ReadInt( wf_entity, "autosnatch", cfg.autosnatch ); + + cfg.paddle_size.x = wf->ReadTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x ); + cfg.paddle_size.y = wf->ReadTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y ); + cfg.paddle_size.z = wf->ReadTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z ); + + const char* paddles = wf->ReadTupleString( wf_entity, "paddles", 0, NULL ); + const char* lift = wf->ReadTupleString( wf_entity, "paddles", 1, NULL ); + + if( paddles && strcmp( paddles, "closed" ) == 0 ) + { + cfg.paddle_position = 1.0; + cfg.paddles = PADDLE_CLOSED; + } + + if( paddles && strcmp( paddles, "open" ) == 0 ) + { + cfg.paddle_position = 0.0; + cfg.paddles = PADDLE_OPEN; + } + + if( lift && strcmp( lift, "up" ) == 0 ) + { + cfg.lift_position = 1.0; + cfg.lift = LIFT_UP; + } + + if( lift && strcmp( lift, "down" ) == 0 ) + { + cfg.lift_position = 0.0; + cfg.lift = LIFT_DOWN; + } + + cfg.grip_stack_size = wf->ReadInt( wf_entity , "stack_size", cfg.grip_stack_size ); + + FixBlocks(); + + // do this at the end to ensure that the blocks are resize correctly + Model::Load(); +} + +void ModelGripper::Save() +{ + Model::Save(); + + wf->WriteTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x ); + wf->WriteTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y ); + wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z ); + + wf->WriteInt( wf_entity , "stack_size", cfg.grip_stack_size ); +} + +void ModelGripper::DataVisualize( Camera* cam ) +{ + /* do nothing */ +} + +void ModelGripper::FixBlocks() +{ + // get rid of the default cube + ClearBlocks(); + + // add three blocks that make the gripper + // base + AddBlockRect( 0, 0, 1.0-cfg.paddle_size.x, 1.0, 1.0 ); + + // left (top) paddle + paddle_left = AddBlockRect( 1.0-cfg.paddle_size.x, 0, cfg.paddle_size.x, cfg.paddle_size.y, cfg.paddle_size.z ); + + // right (bottom) paddle + paddle_right = AddBlockRect( 1.0-cfg.paddle_size.x, 1.0-cfg.paddle_size.y, cfg.paddle_size.x, cfg.paddle_size.y, cfg.paddle_size.z ); + + PositionPaddles(); +} + +// Update the blocks that are the gripper's body +void ModelGripper::PositionPaddles() +{ + UnMap(); + double paddle_center_pos = cfg.paddle_position * (0.5 - cfg.paddle_size.y ); + paddle_left->SetCenterY( paddle_center_pos + cfg.paddle_size.y/2.0 ); + paddle_right->SetCenterY( 1.0 - paddle_center_pos - cfg.paddle_size.y/2.0); + + double paddle_bottom = cfg.lift_position * (1.0 - cfg.paddle_size.z); + double paddle_top = paddle_bottom + cfg.paddle_size.z; + + paddle_left->SetZ( paddle_bottom, paddle_top ); + paddle_right->SetZ( paddle_bottom, paddle_top ); + + Map(); + } + + +void ModelGripper::Update() +{ + // no work to do if we're unsubscribed + if( subs < 1 ) + { + Model::Update(); + return; + } + + float start_paddle_position = cfg.paddle_position; + float start_lift_position = cfg.lift_position; + + switch( cmd ) + { + case CMD_NOOP: + break; + + case CMD_CLOSE: + if( cfg.paddles != PADDLE_CLOSED ) + { + //puts( "closing gripper paddles" ); + cfg.paddles = PADDLE_CLOSING; + } + break; + + case CMD_OPEN: + if( cfg.paddles != PADDLE_OPEN ) + { + //puts( "opening gripper paddles" ); + cfg.paddles = PADDLE_OPENING; + } + break; + + case CMD_UP: + if( cfg.lift != LIFT_UP ) + { + //puts( "raising gripper lift" ); + cfg.lift = LIFT_UPPING; + } + break; + + case CMD_DOWN: + if( cfg.lift != LIFT_DOWN ) + { + //puts( "lowering gripper lift" ); + cfg.lift = LIFT_DOWNING; + } + break; + + default: + printf( "unknown gripper command %d\n",cmd ); + } + + // // move the paddles + if( cfg.paddles == PADDLE_OPENING && !cfg.paddles_stalled ) + { + cfg.paddle_position -= 0.05; + + if( cfg.paddle_position < 0.0 ) // if we're fully open + { + cfg.paddle_position = 0.0; + cfg.paddles = PADDLE_OPEN; // change state + } + + // drop the thing at the head of the stack + if( cfg.grip_stack && + (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit )) + { + Model* head = (Model*)cfg.grip_stack->data; + cfg.grip_stack = g_list_remove( cfg.grip_stack, head ); + + // move it to the new location + head->SetParent( NULL ); + head->SetPose( this->GetGlobalPose() ); + + cfg.close_limit = 1.0; + } + } + + else if( cfg.paddles == PADDLE_CLOSING && !cfg.paddles_stalled ) + { + cfg.paddle_position += 0.05; + //printf( "paddle position %.2f\n", cfg.paddle_position ); + + if( cfg.paddle_position > cfg.close_limit ) // if we're fully closed + { + cfg.paddle_position = cfg.close_limit; + cfg.paddles = PADDLE_CLOSED; // change state + } + } + + switch( cfg.lift ) + { + case LIFT_DOWNING: + cfg.lift_position -= 0.05; + + if( cfg.lift_position < 0.0 ) // if we're fully down + { + cfg.lift_position = 0.0; + cfg.lift = LIFT_DOWN; // change state + } + break; + + case LIFT_UPPING: + cfg.lift_position += 0.05; + + if( cfg.lift_position > 1.0 ) // if we're fully open + { + cfg.lift_position = 1.0; + cfg.lift = LIFT_UP; // change state + } + break; + + case LIFT_DOWN: // nothing to do for these cases + case LIFT_UP: + default: + break; + } + + // if the paddles or lift have changed position + if( start_paddle_position != cfg.paddle_position || + start_lift_position != cfg.lift_position ) + // figure out where the paddles should be + PositionPaddles(); + + + UpdateBreakBeams(); + UpdateContacts(); + + Model::Update(); +} + +ModelGripper::data_t ModelGripper::GetData() +{ + data_t data; + + data.paddles = cfg.paddles; + data.paddle_position = cfg.paddle_position; + data.lift = cfg.lift; + data.lift_position = cfg.lift_position; + data.beam[0] = cfg.beam[0]; + data.beam[1] = cfg.beam[1]; + + data.contact[0] = cfg.contact[0]; + data.contact[1] = cfg.contact[1]; + + if( cfg.grip_stack ) + data.stack_count = g_list_length( cfg.grip_stack ); + else + data.stack_count = 0; + + data.paddles_stalled = cfg.paddles_stalled; + + return data; +} + + +void ModelGripper::SetConfig( config_t & cfg ) +{ + this->cfg = cfg; +} + + +static bool gripper_raytrace_match( Model* hit, + Model* finder, + const void* dummy ) +{ + return( (hit != finder) && hit->vis.gripper_return ); + // can't use the normal relation check, because we may pick things + // up and we must still see them. +} + +void ModelGripper::UpdateBreakBeams() +{ + for( unsigned int index=0; index < 2; index++ ) + { + Pose pz; + + // x location of break beam origin + double inset = cfg.break_beam_inset[index]; + + pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0; + + // y location of break beam origin + pz.y = (1.0 - cfg.paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y)); + + pz.z = 0.0; // TODO + + // break beam local heading + pz.a = -M_PI/2.0; + + // break beam max range + double bbr = + (1.0 - cfg.paddle_position) * (geom.size.y - (geom.size.y * cfg.paddle_size.y * 2.0 )); + + stg_raytrace_result_t sample = + Raytrace( pz, // ray origin + bbr, // range + gripper_raytrace_match, // match func + NULL, // match arg + true ); // ztest + + cfg.beam[index] = sample.mod; + } + + // autosnatch grabs anything that breaks the inner beam + if( cfg.autosnatch ) + { + if( cfg.beam[0] || cfg.beam[1] ) + cmd = CMD_CLOSE; + else + cmd = CMD_OPEN; + } +} + +void ModelGripper::UpdateContacts() +{ + cfg.paddles_stalled = false; // may be changed below + + Pose lpz, rpz; + + // x location of contact sensor origin + lpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ; + rpz.x = ((1.0 - cfg.paddle_size.x) * geom.size.x) - geom.size.x/2.0 ; + +// //double inset = beam ? cfg->inner_break_beam_inset : cfg->outer_break_beam_inset; +// //pz.x = (geom.size.x - inset * geom.size.x) - geom.size.x/2.0; + + // y location of paddle beam origin + + lpz.y = (1.0 - cfg.paddle_position) * + ((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y)); + + rpz.y = (1.0 - cfg.paddle_position) * + -((geom.size.y/2.0) - (geom.size.y*cfg.paddle_size.y)); + + lpz.z = 0.0; // todo + rpz.z = 0.0; + + // paddle beam local heading + lpz.a = 0.0; + rpz.a = 0.0; + + // paddle beam max range + double bbr = cfg.paddle_size.x * geom.size.x; + + stg_raytrace_result_t leftsample = + Raytrace( lpz, // ray origin + bbr, // range + gripper_raytrace_match, // match func + NULL, // match arg + true ); // ztest + + cfg.contact[0] = leftsample.mod; + + stg_raytrace_result_t rightsample = + Raytrace( rpz, // ray origin + bbr, // range + gripper_raytrace_match, // match func + NULL, // match arg + true ); // ztest + + cfg.contact[1] = rightsample.mod; + + if( cfg.contact[0] || cfg.contact[1] ) + { + cfg.paddles_stalled = true;; + + // if( lhit && (lhit == rhit) ) + // { + // //puts( "left and right hit same thing" ); + + if( cfg.paddles == PADDLE_CLOSING ) + { + Model* hit = cfg.contact[0]; + if( !hit ) + hit = cfg.contact[1]; + + if( cfg.grip_stack_size > 0 && + (g_list_length( cfg.grip_stack ) < cfg.grip_stack_size) ) + { + // get the global pose of the gripper for calculations of the gripped object position + // and move it to be right between the paddles + Geom hitgeom = hit->GetGeom(); + //Pose hitgpose = hit->GetGlobalPose(); + + // stg_pose_t pose = {0,0,0}; + // stg_model_local_to_global( lhit, &pose ); + // stg_model_global_to_local( mod, &pose ); + + // // grab the model we hit - very simple grip model for now + hit->SetParent( this ); + hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) ); + + cfg.grip_stack = g_list_prepend( cfg.grip_stack, hit ); + + // // calculate how far closed we can get the paddles now + double puckw = hitgeom.size.y; + double gripperw = geom.size.y; + cfg.close_limit = MAX( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 )); + } + } + } +} + +// int gripper_render_data( stg_model_t* mod, void* userp ) +// { +// //puts( "gripper render data" ); + +// // only draw if someone is using the gripper +// if( mod->subs < 1 ) +// return 0; + +// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_data_fig" ); + +// if( ! fig ) +// { +// fig = stg_model_fig_create( mod, "gripper_data_fig", "top", STG_LAYER_GRIPPERDATA ); +// //stg_rtk_fig_color_rgb32( fig, gripper_col ); +// stg_rtk_fig_color_rgb32( fig, 0xFF0000 ); // red + +// } +// else +// stg_rtk_fig_clear( fig ); + +// //printf( "SUBS %d\n", mod->subs ); + +// stg_gripper_data_t* data = (stg_gripper_data_t*)mod->data; +// assert(data); + +// stg_gripper_config_t *cfg = (stg_gripper_config_t*)mod->cfg; +// assert(cfg); + +// stg_geom_t *geom = &mod->geom; + +// //stg_rtk_fig_rectangle( gui->data, 0,0,0, geom.size.x, geom.size.y, 0 ); + +// // different x location for each beam +// double ibbx = (geom->size.x - cfg->inner_break_beam_inset * geom->size.x) - geom->size.x/2.0; +// double obbx = (geom->size.x - cfg->outer_break_beam_inset * geom->size.x) - geom->size.x/2.0; + +// // common y position +// double bby = +// (1.0-data->paddle_position) * ((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)); + +// // size of the paddle indicator lights +// double led_dx = cfg->paddle_size.y * 0.5 * geom->size.y; + + +// if( data->inner_break_beam ) +// { +// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 1 ); +// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 1 ); +// } +// else +// { +// stg_rtk_fig_line( fig, ibbx, bby, ibbx, -bby ); +// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 0 ); +// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 0 ); +// } + +// if( data->outer_break_beam ) +// { +// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 1 ); +// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 1 ); +// } +// else +// { +// stg_rtk_fig_line( fig, obbx, bby, obbx, -bby ); +// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 0 ); +// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 0 ); +// } + +// // draw the contact indicators +// stg_rtk_fig_rectangle( fig, +// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) - geom->size.x/2.0, +// (1.0 - cfg->paddle_position) * ((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)), +// 0.0, +// cfg->paddle_size.x * geom->size.x, +// cfg->paddle_size.y/6.0 * geom->size.y, +// data->paddle_contacts[0] ); + +// stg_rtk_fig_rectangle( fig, +// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) - geom->size.x/2.0, +// (1.0 - cfg->paddle_position) * -((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)), +// 0.0, +// cfg->paddle_size.x * geom->size.x, +// cfg->paddle_size.y/6.0 * geom->size.y, +// data->paddle_contacts[1] ); + +// //stg_rtk_fig_color_rgb32( fig, gripper_col ); + +// return 0; +// } + + +// int gripper_render_cfg( stg_model_t* mod, void* user ) +// { +// puts( "gripper render cfg" ); +// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_cfg_fig" ); + +// if( ! fig ) +// { +// fig = stg_model_fig_create( mod, "gripper_cfg_fig", "top", +// STG_LAYER_GRIPPERCONFIG ); + +// stg_rtk_fig_color_rgb32( fig, stg_lookup_color( STG_GRIPPER_CFG_COLOR )); +// } +// else +// stg_rtk_fig_clear( fig ); + +// stg_geom_t geom; +// stg_model_get_geom( mod, &geom ); + +// // get the config and make sure it's the right size +// stg_gripper_config_t* cfg = (stg_gripper_config_t*)mod->cfg; +// assert( mod->cfg_len == sizeof(stg_gripper_config_t) ); + +// // different x location for each beam +// double ibbx = (cfg->inner_break_beam_inset) * geom.size.x - geom.size.x/2.0; +// double obbx = (cfg->outer_break_beam_inset) * geom.size.x - geom.size.x/2.0; + +// // common y position +// double bby = +// (1.0-cfg->paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg->paddle_size.y)); + +// // draw the position of the break beam sensors +// stg_rtk_fig_rectangle( fig, ibbx, bby, 0, 0.01, 0.01, 0 ); +// stg_rtk_fig_rectangle( fig, obbx, bby, 0, 0.01, 0.01, 0 ); + +// return 0; //ok +// } + + +// int gripper_startup( stg_model_t* mod ) +// { +// PRINT_DEBUG( "gripper startup" ); +// stg_model_set_watts( mod, STG_GRIPPER_WATTS ); +// return 0; // ok +// } + +// int gripper_shutdown( stg_model_t* mod ) +// { +// PRINT_DEBUG( "gripper shutdown" ); +// stg_model_set_watts( mod, 0 ); + +// // unrender the break beams & lights +// stg_model_fig_clear( mod, "gripper_data_fig" ); +// return 0; // ok +// } + +// void stg_print_gripper_config( stg_gripper_config_t* cfg ) +// { +// char* pstate; +// switch( cfg->paddles ) +// { +// case STG_GRIPPER_PADDLE_OPEN: pstate = "OPEN"; break; +// case STG_GRIPPER_PADDLE_CLOSED: pstate = "CLOSED"; break; +// case STG_GRIPPER_PADDLE_OPENING: pstate = "OPENING"; break; +// case STG_GRIPPER_PADDLE_CLOSING: pstate = "CLOSING"; break; +// default: pstate = "*unknown*"; +// } + +// char* lstate; +// switch( cfg->lift ) +// { +// case STG_GRIPPER_LIFT_UP: lstate = "UP"; break; +// case STG_GRIPPER_LIFT_DOWN: lstate = "DOWN"; break; +// case STG_GRIPPER_LIFT_DOWNING: lstate = "DOWNING"; break; +// case STG_GRIPPER_LIFT_UPPING: lstate = "UPPING"; break; +// default: lstate = "*unknown*"; +// } + +// printf("gripper state: paddles(%s)[%.2f] lift(%s)[%.2f] stall(%s)\n", +// pstate, cfg->paddle_position, lstate, cfg->lift_position, +// cfg->paddles_stalled ? "true" : "false" ); +// } + + +// int gripper_unrender_data( stg_model_t* mod, void* userp ) +// { +// stg_model_fig_clear( mod, "gripper_data_fig" ); +// return 1; // callback just runs one time +// } + +// int gripper_unrender_cfg( stg_model_t* mod, void* userp ) +// { +// stg_model_fig_clear( mod, "gripper_cfg_fig" ); +// return 1; // callback just runs one time +// } + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-15 23:44:51
|
Revision: 7334 http://playerstage.svn.sourceforge.net/playerstage/?rev=7334&view=rev Author: rtv Date: 2009-02-15 23:44:46 +0000 (Sun, 15 Feb 2009) Log Message: ----------- cleaning up gripper and adding visualization Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model_gripper.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-15 23:44:46 UTC (rev 7334) @@ -115,7 +115,7 @@ void Dock() { // close the grippers so they can be pushed into the charger - ModelGripper::data_t gripper_data = gripper->GetData(); + ModelGripper::config_t gripper_data = gripper->GetConfig(); if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED ) gripper->CommandClose(); @@ -183,7 +183,7 @@ pos->SetXSpeed( 0.0 ); // once we have backed off a bit, open and lower the gripper - ModelGripper::data_t gripper_data = gripper->GetData(); + ModelGripper::config_t gripper_data = gripper->GetConfig(); if( charger_range > gripper_distance ) { if( gripper_data.paddles != ModelGripper::PADDLE_OPEN ) @@ -284,7 +284,7 @@ { if( verbose ) puts( "Cruise" ); - ModelGripper::data_t gdata = gripper->GetData(); + ModelGripper::config_t gdata = gripper->GetConfig(); //avoidcount = 0; pos->SetXSpeed( cruisespeed ); @@ -303,7 +303,7 @@ if( y < 0 ) y = 0; double a_goal = - dtor( ( pos->GetFlagCount() || gdata.stack_count ) ? have[y][x] : need[y][x] ); + dtor( ( pos->GetFlagCount() || gdata.gripped ) ? have[y][x] : need[y][x] ); // if we are low on juice - find the direction to the recharger instead if( Hungry() ) @@ -481,8 +481,8 @@ static int GripperUpdate( ModelGripper* grip, Robot* robot ) { - ModelGripper::data_t gdata = grip->GetData(); - + ModelGripper::config_t gdata = grip->GetConfig(); + printf( "BREAKBEAMS %s %s\n", gdata.beam[0] ? gdata.beam[0]->Token() : "<null>", gdata.beam[1] ? gdata.beam[1]->Token() : "<null>" ); Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/libstage/gl.cc 2009-02-15 23:44:46 UTC (rev 7334) @@ -50,6 +50,11 @@ glEnd(); } +void Stg::Gl::draw_centered_rect( float x, float y, float dx, float dy ) +{ + glRectf( x-0.5*dx, y-0.5*dy, x+0.5*dx, y+0.5*dy ); +} + void Stg::Gl::draw_vector( double x, double y, double z ) { glBegin( GL_LINES ); Modified: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/libstage/model_gripper.cc 2009-02-15 23:44:46 UTC (rev 7334) @@ -27,10 +27,12 @@ gripper ( # gripper properties - <none> + paddle_size [ 0.66 0.1 0.4 ] + paddle_state [ "open" "down" ] + autosnatch 0 # model properties - size [0.12 0.28] + size [ 0.2 0.3 0.2 ] ) @endverbatim @@ -38,6 +40,12 @@ @par Details +- autosnatch < 0 or 1>\n + iff 1, gripper will close automatically when break beams are broken +- paddle_size [ <float x> <float y < float z> ]\n + Gripper paddle size as a proportion of the model's body size (0.0 to 1.0) +- paddle_state [ <string open/close> <string up/down> ]\n + Gripper arms state, either "open" or "closed", and lift state, either "up" or "down" */ @@ -47,6 +55,9 @@ #include "worldfile.hh" using namespace Stg; +#include "option.hh" +Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true, NULL ); + // TODO - simulate energy use when moving grippers ModelGripper::ModelGripper( World* world, @@ -66,8 +77,7 @@ cfg.lift_position = 0.0; cfg.paddles_stalled = false; cfg.autosnatch = false; - cfg.grip_stack = NULL; - cfg.grip_stack_size = 1; + cfg.gripped = NULL; // place the break beam sensors at 1/4 and 3/4 the length of the paddle cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x; @@ -88,10 +98,10 @@ geom.size.y = 0.3; geom.size.z = 0.2; SetGeom( geom ); - - //Startup(); PositionPaddles(); + + registerOption( &showData ); } ModelGripper::~ModelGripper() @@ -108,8 +118,8 @@ cfg.paddle_size.y = wf->ReadTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y ); cfg.paddle_size.z = wf->ReadTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z ); - const char* paddles = wf->ReadTupleString( wf_entity, "paddles", 0, NULL ); - const char* lift = wf->ReadTupleString( wf_entity, "paddles", 1, NULL ); + const char* paddles = wf->ReadTupleString( wf_entity, "paddle_state", 0, NULL ); + const char* lift = wf->ReadTupleString( wf_entity, "paddle_state", 1, NULL ); if( paddles && strcmp( paddles, "closed" ) == 0 ) { @@ -135,8 +145,6 @@ cfg.lift = LIFT_DOWN; } - cfg.grip_stack_size = wf->ReadInt( wf_entity , "stack_size", cfg.grip_stack_size ); - FixBlocks(); // do this at the end to ensure that the blocks are resize correctly @@ -149,14 +157,10 @@ wf->WriteTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x ); wf->WriteTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y ); - wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z ); - - wf->WriteInt( wf_entity , "stack_size", cfg.grip_stack_size ); -} + wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z ); -void ModelGripper::DataVisualize( Camera* cam ) -{ - /* do nothing */ + wf->WriteTupleString( wf_entity, "paddle_state", 0, (cfg.paddles == PADDLE_CLOSED ) ? "closed" : "open" ); + wf->WriteTupleString( wf_entity, "paddle_state", 1, (cfg.lift == LIFT_UP ) ? "up" : "down" ); } void ModelGripper::FixBlocks() @@ -259,16 +263,15 @@ cfg.paddles = PADDLE_OPEN; // change state } - // drop the thing at the head of the stack - if( cfg.grip_stack && + + // drop the thing we're carrying + if( cfg.gripped && (cfg.paddle_position == 0.0 || cfg.paddle_position < cfg.close_limit )) { - Model* head = (Model*)cfg.grip_stack->data; - cfg.grip_stack = g_list_remove( cfg.grip_stack, head ); - // move it to the new location - head->SetParent( NULL ); - head->SetPose( this->GetGlobalPose() ); + cfg.gripped->SetParent( NULL ); + cfg.gripped->SetPose( this->GetGlobalPose() ); + cfg.gripped = NULL; cfg.close_limit = 1.0; } @@ -327,37 +330,8 @@ Model::Update(); } -ModelGripper::data_t ModelGripper::GetData() -{ - data_t data; - - data.paddles = cfg.paddles; - data.paddle_position = cfg.paddle_position; - data.lift = cfg.lift; - data.lift_position = cfg.lift_position; - data.beam[0] = cfg.beam[0]; - data.beam[1] = cfg.beam[1]; - data.contact[0] = cfg.contact[0]; - data.contact[1] = cfg.contact[1]; - - if( cfg.grip_stack ) - data.stack_count = g_list_length( cfg.grip_stack ); - else - data.stack_count = 0; - - data.paddles_stalled = cfg.paddles_stalled; - return data; -} - - -void ModelGripper::SetConfig( config_t & cfg ) -{ - this->cfg = cfg; -} - - static bool gripper_raytrace_match( Model* hit, Model* finder, const void* dummy ) @@ -473,8 +447,7 @@ if( !hit ) hit = cfg.contact[1]; - if( cfg.grip_stack_size > 0 && - (g_list_length( cfg.grip_stack ) < cfg.grip_stack_size) ) + if( cfg.gripped == NULL ) // if we're not carrying something already { // get the global pose of the gripper for calculations of the gripped object position // and move it to be right between the paddles @@ -489,7 +462,7 @@ hit->SetParent( this ); hit->SetPose( Pose(0,0, -1.0 * geom.size.z ,0) ); - cfg.grip_stack = g_list_prepend( cfg.grip_stack, hit ); + cfg.gripped = hit; // // calculate how far closed we can get the paddles now double puckw = hitgeom.size.y; @@ -500,189 +473,76 @@ } } -// int gripper_render_data( stg_model_t* mod, void* userp ) -// { -// //puts( "gripper render data" ); -// // only draw if someone is using the gripper -// if( mod->subs < 1 ) -// return 0; - -// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_data_fig" ); +void ModelGripper::DataVisualize( Camera* cam ) +{ + // only draw if someone is using the gripper + if( subs < 1 ) + return; -// if( ! fig ) -// { -// fig = stg_model_fig_create( mod, "gripper_data_fig", "top", STG_LAYER_GRIPPERDATA ); -// //stg_rtk_fig_color_rgb32( fig, gripper_col ); -// stg_rtk_fig_color_rgb32( fig, 0xFF0000 ); // red - -// } -// else -// stg_rtk_fig_clear( fig ); + if( ! showData ) + return; -// //printf( "SUBS %d\n", mod->subs ); - -// stg_gripper_data_t* data = (stg_gripper_data_t*)mod->data; -// assert(data); - -// stg_gripper_config_t *cfg = (stg_gripper_config_t*)mod->cfg; -// assert(cfg); + // outline the sensor lights in black + PushColor( 0,0,0,1.0 ); // black + glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); -// stg_geom_t *geom = &mod->geom; - -// //stg_rtk_fig_rectangle( gui->data, 0,0,0, geom.size.x, geom.size.y, 0 ); - -// // different x location for each beam -// double ibbx = (geom->size.x - cfg->inner_break_beam_inset * geom->size.x) - geom->size.x/2.0; -// double obbx = (geom->size.x - cfg->outer_break_beam_inset * geom->size.x) - geom->size.x/2.0; - -// // common y position -// double bby = -// (1.0-data->paddle_position) * ((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)); - -// // size of the paddle indicator lights -// double led_dx = cfg->paddle_size.y * 0.5 * geom->size.y; - - -// if( data->inner_break_beam ) -// { -// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 1 ); -// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 1 ); -// } -// else -// { -// stg_rtk_fig_line( fig, ibbx, bby, ibbx, -bby ); -// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 0 ); -// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 0 ); -// } - -// if( data->outer_break_beam ) -// { -// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 1 ); -// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 1 ); -// } -// else -// { -// stg_rtk_fig_line( fig, obbx, bby, obbx, -bby ); -// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 0 ); -// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 0 ); -// } - -// // draw the contact indicators -// stg_rtk_fig_rectangle( fig, -// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) - geom->size.x/2.0, -// (1.0 - cfg->paddle_position) * ((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)), -// 0.0, -// cfg->paddle_size.x * geom->size.x, -// cfg->paddle_size.y/6.0 * geom->size.y, -// data->paddle_contacts[0] ); - -// stg_rtk_fig_rectangle( fig, -// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) - geom->size.x/2.0, -// (1.0 - cfg->paddle_position) * -((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)), -// 0.0, -// cfg->paddle_size.x * geom->size.x, -// cfg->paddle_size.y/6.0 * geom->size.y, -// data->paddle_contacts[1] ); - -// //stg_rtk_fig_color_rgb32( fig, gripper_col ); + // different x location for each beam + double ibbx = (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) - geom.size.x/2.0; + double obbx = (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) - geom.size.x/2.0; + + // common y position + double invp = 1.0 - cfg.paddle_position; + double bby = + invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y)); + + // // size of the paddle indicator lights + double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y; + + // paddle break beams + Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx ); + Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx ); + Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx ); + Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx ); + + // paddle contacts + double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) - geom.size.x/2.0; + double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y); + double plen = cfg.paddle_size.x * geom.size.x; + double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y; + + Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth ); + Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth ); + + // if the gripper detects anything, fill the lights in with yellow + if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] ) + { + PushColor( 1,1,0,1.0 ); // yellow + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + if( cfg.contact[0] ) + Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth ); + + if( cfg.contact[1] ) + Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth ); + + if( cfg.beam[0] ) + { + Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx ); + Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx ); + } + + if( cfg.beam[1] ) + { + Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx ); + Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx ); + } -// return 0; -// } + PopColor(); // yellow + } + + PopColor(); // black + } -// int gripper_render_cfg( stg_model_t* mod, void* user ) -// { -// puts( "gripper render cfg" ); -// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_cfg_fig" ); - -// if( ! fig ) -// { -// fig = stg_model_fig_create( mod, "gripper_cfg_fig", "top", -// STG_LAYER_GRIPPERCONFIG ); - -// stg_rtk_fig_color_rgb32( fig, stg_lookup_color( STG_GRIPPER_CFG_COLOR )); -// } -// else -// stg_rtk_fig_clear( fig ); - -// stg_geom_t geom; -// stg_model_get_geom( mod, &geom ); - -// // get the config and make sure it's the right size -// stg_gripper_config_t* cfg = (stg_gripper_config_t*)mod->cfg; -// assert( mod->cfg_len == sizeof(stg_gripper_config_t) ); - -// // different x location for each beam -// double ibbx = (cfg->inner_break_beam_inset) * geom.size.x - geom.size.x/2.0; -// double obbx = (cfg->outer_break_beam_inset) * geom.size.x - geom.size.x/2.0; - -// // common y position -// double bby = -// (1.0-cfg->paddle_position) * ((geom.size.y/2.0)-(geom.size.y*cfg->paddle_size.y)); - -// // draw the position of the break beam sensors -// stg_rtk_fig_rectangle( fig, ibbx, bby, 0, 0.01, 0.01, 0 ); -// stg_rtk_fig_rectangle( fig, obbx, bby, 0, 0.01, 0.01, 0 ); - -// return 0; //ok -// } - - -// int gripper_startup( stg_model_t* mod ) -// { -// PRINT_DEBUG( "gripper startup" ); -// stg_model_set_watts( mod, STG_GRIPPER_WATTS ); -// return 0; // ok -// } - -// int gripper_shutdown( stg_model_t* mod ) -// { -// PRINT_DEBUG( "gripper shutdown" ); -// stg_model_set_watts( mod, 0 ); - -// // unrender the break beams & lights -// stg_model_fig_clear( mod, "gripper_data_fig" ); -// return 0; // ok -// } - -// void stg_print_gripper_config( stg_gripper_config_t* cfg ) -// { -// char* pstate; -// switch( cfg->paddles ) -// { -// case STG_GRIPPER_PADDLE_OPEN: pstate = "OPEN"; break; -// case STG_GRIPPER_PADDLE_CLOSED: pstate = "CLOSED"; break; -// case STG_GRIPPER_PADDLE_OPENING: pstate = "OPENING"; break; -// case STG_GRIPPER_PADDLE_CLOSING: pstate = "CLOSING"; break; -// default: pstate = "*unknown*"; -// } - -// char* lstate; -// switch( cfg->lift ) -// { -// case STG_GRIPPER_LIFT_UP: lstate = "UP"; break; -// case STG_GRIPPER_LIFT_DOWN: lstate = "DOWN"; break; -// case STG_GRIPPER_LIFT_DOWNING: lstate = "DOWNING"; break; -// case STG_GRIPPER_LIFT_UPPING: lstate = "UPPING"; break; -// default: lstate = "*unknown*"; -// } - -// printf("gripper state: paddles(%s)[%.2f] lift(%s)[%.2f] stall(%s)\n", -// pstate, cfg->paddle_position, lstate, cfg->lift_position, -// cfg->paddles_stalled ? "true" : "false" ); -// } - - -// int gripper_unrender_data( stg_model_t* mod, void* userp ) -// { -// stg_model_fig_clear( mod, "gripper_data_fig" ); -// return 1; // callback just runs one time -// } - -// int gripper_unrender_cfg( stg_model_t* mod, void* userp ) -// { -// stg_model_fig_clear( mod, "gripper_cfg_fig" ); -// return 1; // callback just runs one time -// } - Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/libstage/model_laser.cc 2009-02-15 23:44:46 UTC (rev 7334) @@ -24,7 +24,6 @@ static const bool DEFAULT_FILLED = true; static const stg_watts_t DEFAULT_WATTS = 17.5; static const Size DEFAULT_SIZE( 0.15, 0.15, 0.2 ); -static const stg_meters_t DEFAULT_MINRANGE = 0.0; static const stg_meters_t DEFAULT_MAXRANGE = 8.0; static const stg_radians_t DEFAULT_FOV = M_PI; static const unsigned int DEFAULT_SAMPLES = 180; @@ -35,6 +34,8 @@ //TODO make instance attempt to register an option (as customvisualizations do) Option ModelLaser::showLaserData( "Laser scans", "show_laser", "", true, NULL ); Option ModelLaser::showLaserStrikes( "Laser strikes", "show_laser_strikes", "", false, NULL ); +Option ModelLaser::showLaserFov( "Laser FOV", "show_laser_fov", "", false, NULL ); +Option ModelLaser::showLaserBeams( "Laser beams", "show_laser_beams", "", false, NULL ); /** @ingroup model @@ -52,7 +53,6 @@ ( # laser properties samples 180 - range_min 0.0 range_max 8.0 fov 3.14159 resolution 1 @@ -67,8 +67,6 @@ - samples <int>\n the number of laser samples per scan -- range_min <float>\n - the minimum range reported by the scanner, in meters. The scanner will detect objects closer than this, but report their range as the minimum. - range_max <float>\n the maximum range reported by the scanner, in meters. The scanner will not detect objects beyond this range. - fov <float>\n @@ -84,7 +82,6 @@ data_dirty( true ), samples( NULL ), // don't allocate sample buffer memory until Update() is called sample_count( DEFAULT_SAMPLES ), - range_min( DEFAULT_MINRANGE ), range_max( DEFAULT_MAXRANGE ), fov( DEFAULT_FOV ), resolution( DEFAULT_RESOLUTION ) @@ -113,6 +110,8 @@ registerOption( &showLaserData ); registerOption( &showLaserStrikes ); + registerOption( &showLaserFov ); + registerOption( &showLaserBeams ); } @@ -128,7 +127,6 @@ void ModelLaser::Load( void ) { sample_count = wf->ReadInt( wf_entity, "samples", sample_count ); - range_min = wf->ReadLength( wf_entity, "range_min", range_min); range_max = wf->ReadLength( wf_entity, "range_max", range_max ); fov = wf->ReadAngle( wf_entity, "fov", fov ); resolution = wf->ReadInt( wf_entity, "resolution", resolution ); @@ -149,7 +147,6 @@ { stg_laser_cfg_t cfg; cfg.sample_count = sample_count; - cfg.range_bounds.min = range_min; cfg.range_bounds.max = range_max; cfg.fov = fov; cfg.resolution = resolution; @@ -159,7 +156,6 @@ void ModelLaser::SetConfig( stg_laser_cfg_t cfg ) { - range_min = cfg.range_bounds.min; range_max = cfg.range_bounds.max; fov = cfg.fov; resolution = cfg.resolution; @@ -309,8 +305,8 @@ { if( ! (samples && sample_count) ) return; - - if ( ! (showLaserData || showLaserStrikes) ) + + if ( ! (showLaserData || showLaserStrikes || showLaserFov || showLaserBeams ) ) return; glPushMatrix(); @@ -379,7 +375,43 @@ glDrawArrays( GL_POINTS, 0, sample_count+1 ); PopColor(); } + + if( showLaserFov ) + { + for( unsigned int s=0; s<sample_count; s++ ) + { + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + pts[2*s+2] = (float)(range_max * cos(ray_angle) ); + pts[2*s+3] = (float)(range_max * sin(ray_angle) ); + } + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + PushColor( 0, 0, 1, 0.5 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + PopColor(); + // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + } + if( showLaserBeams ) + { + PushColor( 0, 0, 1, 0.5 ); + glBegin( GL_LINES ); + + for( unsigned int s=0; s<sample_count; s++ ) + { + + glVertex2f( 0,0 ); + double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; + glVertex2f( samples[s].range * cos(ray_angle), + samples[s].range * sin(ray_angle) ); + + } + glEnd(); + PopColor(); + } + + glDepthMask( GL_TRUE ); glEndList(); } // end if ( data_dirty ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/libstage/stage.hh 2009-02-15 23:44:46 UTC (rev 7334) @@ -466,6 +466,8 @@ void draw_octagon( float w, float h, float m ); void draw_vector( double x, double y, double z ); void draw_origin( double len ); + /** Draws a rectangle with center at x,y, with sides of length dx,dy */ + void draw_centered_rect( float x, float y, float dx, float dy ); } @@ -2233,7 +2235,8 @@ // LASER MODEL -------------------------------------------------------- - + + // TODO - move these into the class definition, like the gripper /** laser sample packet */ typedef struct @@ -2261,12 +2264,14 @@ stg_laser_sample_t* samples; uint32_t sample_count; - stg_meters_t range_min, range_max; + stg_meters_t range_max; stg_radians_t fov; uint32_t resolution; static Option showLaserData; static Option showLaserStrikes; + static Option showLaserFov; + static Option showLaserBeams; public: static const char* typestr; @@ -2304,6 +2309,14 @@ { public: + // class Viz : public CustomVisualizer +// { + + +// }; + +// static Viz viz; + enum paddle_state_t { PADDLE_OPEN = 0, // default state PADDLE_CLOSED, @@ -2326,7 +2339,7 @@ CMD_DOWN }; - + /** gripper configuration */ struct config_t @@ -2339,12 +2352,11 @@ double paddle_position; ///< 0.0 = full open, 1.0 full closed double lift_position; ///< 0.0 = full down, 1.0 full up + Model* gripped; + bool paddles_stalled; // true iff some solid object stopped // the paddles closing or opening - GList*grip_stack; ///< stack of items gripped - int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited - double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically @@ -2353,28 +2365,7 @@ Model* beam[2]; ///< points to a model detected by the beams Model* contact[2]; ///< pointers to a model detected by the contacts }; - - - /** gripper data packet - */ - struct data_t - { - paddle_state_t paddles; - lift_state_t lift; - - double paddle_position; ///< 0.0 = full open, 1.0 full closed - double lift_position; ///< 0.0 = full down, 1.0 full up - - - stg_bool_t paddles_stalled; // true iff some solid object stopped - // the paddles closing or opening - - Model* beam[2]; ///< points to a model detected by the beams - Model* contact[2]; ///< pointers to a model detected by the contacts - - int stack_count; ///< number of objects in stack - }; - + private: virtual void Update(); virtual void DataVisualize( Camera* cam ); @@ -2390,6 +2381,8 @@ Block* paddle_left; Block* paddle_right; + static Option showData; + public: static const char* typestr; static const Size size; @@ -2403,13 +2396,11 @@ virtual void Load(); virtual void Save(); - void SetConfig( config_t & newcfg ); - - /** Returns the static state of the gripper */ + /** Configure the gripper */ + void SetConfig( config_t & newcfg ){ this->cfg = cfg; FixBlocks(); } + + /** Returns the state of the gripper .*/ config_t GetConfig(){ return cfg; }; - - /** Returns the dynamic state of the gripper */ - data_t GetData(); /** Set the current activity of the gripper. */ void SetCommand( cmd_t cmd ) { this->cmd = cmd; } Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-13 18:55:58 UTC (rev 7333) +++ code/stage/trunk/worlds/fasr.world 2009-02-15 23:44:46 UTC (rev 7334) @@ -22,9 +22,9 @@ ( size [ 1379.000 856.000 ] - center [ 2.468 4.122 ] + center [ 0.783 5.056 ] rotate [ 0 0 ] - scale 91.508 + scale 355.764 pcam_loc [ 0 -4.000 2.000 ] pcam_angle [ 70.000 0 ] @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 180 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" joules 100000 joules_capacity 400000 @@ -151,7 +151,7 @@ puck( pose [ 0.549 2.367 0 0 ] ) puck( pose [ 0.162 2.983 0 0 ] ) puck( pose [ 1.067 3.367 0 0 ] ) -puck( pose [ 1.577 3.488 0 0 ] ) +puck( pose [ 0.320 4.907 0 0 ] ) autorob( pose [5.418 7.478 0 -163.478] joules 300000 ) autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) @@ -165,7 +165,7 @@ autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) -autorob( pose [4.127 5.388 0 -147.713] joules 400000 ) +autorob( pose [1.095 5.276 0 -147.713] joules 400000 ) autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-16 03:01:12
|
Revision: 7336 http://playerstage.svn.sourceforge.net/playerstage/?rev=7336&view=rev Author: rtv Date: 2009-02-16 01:59:06 +0000 (Mon, 16 Feb 2009) Log Message: ----------- working on simplifying GUI option code Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-16 01:59:06 UTC (rev 7336) @@ -4,7 +4,6 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs ancestor.cc - model_gripper.cc block.cc blockgroup.cc camera.cc @@ -21,6 +20,7 @@ model_camera.cc model_fiducial.cc model_getset.cc + model_gripper.cc model_laser.cc model_load.cc model_position.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/model.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -1019,3 +1019,11 @@ return NULL; } + +void Model::RegisterOption( Option* opt ) +{ + drawOptions.push_back( opt ); + + if( world->IsGUI() ) + world->RegisterOption( opt ); +} Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/model_draw.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -263,7 +263,11 @@ Canvas* canvas = world_gui->GetCanvas(); std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); if( i == canvas->_custom_options.end() ) { - Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui ); + Option* op = new Option( custom_visual->name(), + custom_visual->name(), + "", + true, + world_gui ); canvas->_custom_options[ custom_visual->name() ] = op; registerOption( op ); } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/model_laser.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -108,10 +108,12 @@ if( world->IsGUI() ) data_dl = glGenLists(1); - registerOption( &showLaserData ); - registerOption( &showLaserStrikes ); - registerOption( &showLaserFov ); - registerOption( &showLaserBeams ); + RegisterOption( &showLaserData ); + RegisterOption( &showLaserStrikes ); + RegisterOption( &showLaserFov ); + RegisterOption( &showLaserBeams ); + + //AddCustomVisualizer( new LaserScanVis( this )); } @@ -420,3 +422,10 @@ glPopMatrix(); } + + +// void ModelLaser::LaserScanVis::DataVisualize( Camera* cam ) +// { +// puts( "LSV DataVisualize" ); +// laser->DataVisualize( cam ); +// } Modified: code/stage/trunk/libstage/option.cc =================================================================== --- code/stage/trunk/libstage/option.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/option.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -13,7 +13,8 @@ shortcut( key ), menu( NULL ), menuCb( NULL ), -_world( world ) +_world( world ), +htname( strdup(n.c_str()) ) { } Fl_Menu_Item* getMenuItem( Fl_Menu_* menu, int i ) { Modified: code/stage/trunk/libstage/option.hh =================================================================== --- code/stage/trunk/libstage/option.hh 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/option.hh 2009-02-16 01:59:06 UTC (rev 7336) @@ -55,6 +55,8 @@ static void toggleCb( Fl_Widget* w, void* p ); void Load( Worldfile* wf, int section ); void Save( Worldfile* wf, int section ); + + const char* htname; }; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/stage.hh 2009-02-16 01:59:06 UTC (rev 7336) @@ -891,6 +891,7 @@ stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps + GHashTable* option_table; ///< GUI options (toggles) registered by models GList* powerpack_list; ///< List of all the powerpacks attached to models in the world GList* ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in ms @@ -1051,6 +1052,9 @@ /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() { return updates; } + + /// Register an Option for pickup by the GUI + void RegisterOption( Option* opt ); }; class Block @@ -1376,7 +1380,7 @@ stg_usec_t real_time_of_last_update; void UpdateOptions(); - + // static callback functions static void windowCb( Fl_Widget* w, void* p ); static void fileLoadCb( Fl_Widget* w, void* p ); @@ -1507,8 +1511,7 @@ virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory) }; - - + /* Hooks for attaching special callback functions (not used as variables - we just need unique addresses for them.) */ class CallbackHooks @@ -1534,46 +1537,6 @@ void Load( Worldfile* wf, int wf_entity ); }; - // class Option { - // private: - // friend bool compare( const Option* lhs, const Option* rhs ); - - // std::string optName; - // bool value; - // /** worldfile entry string for loading and saving this value */ - // std::string wf_token; - // std::string shortcut; - // Fl_Menu_* menu; - // int menuIndex; - // Fl_Callback* menuCb; - // Fl_Widget* menuCbWidget; - - // public: - // Option( std::string n, std::string tok, std::string key, bool v, WorldGui* worldgui ); - - // const std::string name() const { return optName; } - // inline bool val() const { return value; } - // inline operator bool() { return val(); } - // inline bool operator<( const Option& rhs ) const - // { return optName<rhs.optName; } - // void set( bool val ); - // void invert() { set( !value ); } - - // // Comparator to dereference Option pointers and compare their strings - // struct optComp { - // inline bool operator()( const Option* lhs, const Option* rhs ) const - // { return lhs->operator<(*rhs); } - // }; - - - // void createMenuItem( Fl_Menu_Bar* menu, std::string path ); - // void menuCallback( Fl_Callback* cb, Fl_Widget* w ); - // static void toggleCb( Fl_Widget* w, void* p ); - // void Load( Worldfile* wf, int section ); - // void Save( Worldfile* wf, int section ); - // }; - - /// %Model class class Model : public Ancestor { @@ -1720,8 +1683,8 @@ protected: /// Register an Option for pickup by the GUI - void registerOption( Option* opt ) - { drawOptions.push_back( opt ); } + void RegisterOption( Option* opt ); + void registerOption( Option* opt ) { RegisterOption( opt) ; }; GList* AppendTouchingModels( GList* list ); //void AddTouchingModelsToList( GList* list ); @@ -1872,8 +1835,10 @@ virtual ~Model(); void Say( const char* str ); - /** Attach a user supplied visualization to a model */ + + /** Attach a user supplied visualization to a model. */ void AddCustomVisualizer( CustomVisualizer* custom_visual ); + /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); @@ -2272,7 +2237,27 @@ static Option showLaserStrikes; static Option showLaserFov; static Option showLaserBeams; - + +// class LaserScanVis : public CustomVisualizer +// { +// public: +// LaserScanVis( ModelLaser* laser ) : +// CustomVisualizer(), +// laser( laser ) +// { /* nothing to do */ }; + +// virtual void DataVisualize( Camera* cam ); + +// // rtv - surely a static string member would be easier here? +// //must return a name for visualization (careful not to return stack-memory) + +// virtual const std::string& name() { return "LaserScanVisName"; } ; + +// private: +// ModelLaser* laser; +// }; + + public: static const char* typestr; // constructor Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/world.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -50,6 +50,7 @@ #include "file_manager.hh" #include "worldfile.hh" #include "region.hh" +#include "option.hh" using namespace Stg; // static data members @@ -95,6 +96,7 @@ extent(), graphics( false ), interval_sim( (stg_usec_t)thousand * interval_sim ), + option_table( g_hash_table_new( g_str_hash, g_str_equal ) ), ray_list( NULL ), sim_time( 0 ), superregions( g_hash_table_new( (GHashFunc)PointIntHash, @@ -945,3 +947,9 @@ { powerpack_list = g_list_remove( powerpack_list, pp ); } + +/// Register an Option for pickup by the GUI +void World:: RegisterOption( Option* opt ) +{ + g_hash_table_insert( option_table, (void*)opt->htname, opt ); +} Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/libstage/worldgui.cc 2009-02-16 01:59:06 UTC (rev 7336) @@ -519,6 +519,12 @@ } +void list_option( char* name, Option* opt, void* dummy ) +{ + printf( "option %s @ %p\n", name, opt ); + +} + void WorldGui::viewOptionsCb( Fl_Widget* w, void* p ) { WorldGui* worldGui = static_cast<WorldGui*>( p ); @@ -538,6 +544,8 @@ { worldGui->oDlg->show(); // bring it to front } + + //g_hash_table_foreach( worldGui->option_table, (GHFunc)list_option, NULL ); } void WorldGui::optionsDlgCb( Fl_Widget* w, void* p ) { Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-02-16 00:02:13 UTC (rev 7335) +++ code/stage/trunk/worlds/fasr.world 2009-02-16 01:59:06 UTC (rev 7336) @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 180 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" joules 100000 joules_capacity 400000 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-24 23:17:25
|
Revision: 7355 http://playerstage.svn.sourceforge.net/playerstage/?rev=7355&view=rev Author: rtv Date: 2009-02-24 23:17:20 +0000 (Tue, 24 Feb 2009) Log Message: ----------- fixed gripper bug reported by Pante a Modified Paths: -------------- code/stage/trunk/examples/ctrl/CMakeLists.txt code/stage/trunk/libstage/model_gripper.cc code/stage/trunk/libstage/option.cc Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt =================================================================== --- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-02-24 09:49:34 UTC (rev 7354) +++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-02-24 23:17:20 UTC (rev 7355) @@ -5,6 +5,7 @@ sink source wander + justATest ) Modified: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc 2009-02-24 09:49:34 UTC (rev 7354) +++ code/stage/trunk/libstage/model_gripper.cc 2009-02-24 23:17:20 UTC (rev 7355) @@ -257,7 +257,7 @@ } // // move the paddles - if( cfg.paddles == PADDLE_OPENING && !cfg.paddles_stalled ) + if( cfg.paddles == PADDLE_OPENING )// && !cfg.paddles_stalled ) { cfg.paddle_position -= 0.05; @@ -281,7 +281,7 @@ } } - else if( cfg.paddles == PADDLE_CLOSING && !cfg.paddles_stalled ) + else if( cfg.paddles == PADDLE_CLOSING ) //&& !cfg.paddles_stalled ) { cfg.paddle_position += 0.05; //printf( "paddle position %.2f\n", cfg.paddle_position ); Modified: code/stage/trunk/libstage/option.cc =================================================================== --- code/stage/trunk/libstage/option.cc 2009-02-24 09:49:34 UTC (rev 7354) +++ code/stage/trunk/libstage/option.cc 2009-02-24 23:17:20 UTC (rev 7355) @@ -6,20 +6,27 @@ using namespace Stg; -Option::Option( std::string n, std::string tok, std::string key, bool v, World* world ) : -optName( n ), -value( v ), -wf_token( tok ), -shortcut( key ), -menu( NULL ), -menuCb( NULL ), -_world( world ), -htname( strdup(n.c_str()) ) -{ } +Option::Option( std::string n, + std::string tok, + std::string key, + bool v, + World* world ) : + optName( n ), + value( v ), + wf_token( tok ), + shortcut( key ), + menu( NULL ), + menuCb( NULL ), + _world( world ), + htname( strdup(n.c_str()) ) +{ + /* do nothing */ +} -Fl_Menu_Item* getMenuItem( Fl_Menu_* menu, int i ) { - const Fl_Menu_Item* mArr = menu->menu(); - return const_cast<Fl_Menu_Item*>( &mArr[ i ] ); +Fl_Menu_Item* getMenuItem( Fl_Menu_* menu, int i ) +{ + const Fl_Menu_Item* mArr = menu->menu(); + return const_cast<Fl_Menu_Item*>( &mArr[ i ] ); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-03 18:40:34
|
Revision: 7360 http://playerstage.svn.sourceforge.net/playerstage/?rev=7360&view=rev Author: rtv Date: 2009-03-03 18:40:23 +0000 (Tue, 03 Mar 2009) Log Message: ----------- fixed thread pool implementation Modified Paths: -------------- code/stage/trunk/CMakeLists.txt code/stage/trunk/config.h.in code/stage/trunk/libstage/model_gripper.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/CMakeLists.txt =================================================================== --- code/stage/trunk/CMakeLists.txt 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/CMakeLists.txt 2009-03-03 18:40:23 UTC (rev 7360) @@ -14,6 +14,8 @@ OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF) OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF) +OPTION (BUILD_GUI "WARNING: turning this off breaks the build right now. Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) + cmake_minimum_required( VERSION 2.4 FATAL_ERROR ) IF (CMAKE_MAJOR_VERSION EQUAL 2 AND NOT CMAKE_MINOR_VERSION LESS 6) @@ -75,6 +77,10 @@ MESSAGE( ${INDENT} "Libpng not detected" ) ENDIF( LIBPNG_FOUND ) +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. ## so we can't do this stuff #find_package( FLTK REQUIRED ) @@ -125,6 +131,8 @@ SET (FLTK_FOUND TRUE) +ENDIF( BUILD_GUI ) + #MESSAGE( ${INDENT} "Checking for OpenGL" ) find_package( OpenGL REQUIRED ) IF( NOT OPENGL_GLU_FOUND ) @@ -157,6 +165,8 @@ # SET(APPLE_LIBRARIES "-Wl,-dylib_file,/System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib:/System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib") #ENDIF (APPLE) +MESSAGE( STATUS "Installation directory CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}" ) + # all targets need these include directories include_directories( . libstage Modified: code/stage/trunk/config.h.in =================================================================== --- code/stage/trunk/config.h.in 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/config.h.in 2009-03-03 18:40:23 UTC (rev 7360) @@ -6,5 +6,7 @@ #define APIVERSION "@APIVERSION@" #define INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@" +#cmakedefine BUILD_GUI + #endif Modified: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/libstage/model_gripper.cc 2009-03-03 18:40:23 UTC (rev 7360) @@ -93,6 +93,9 @@ FixBlocks(); + // Update() is not reentrant + thread_safe = false; + // default size Geom geom; geom.pose.x = 0.0; @@ -205,6 +208,8 @@ void ModelGripper::Update() { + //return; + // no work to do if we're unsubscribed if( subs < 1 ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/libstage/stage.hh 2009-03-03 18:40:23 UTC (rev 7360) @@ -897,7 +897,11 @@ stg_usec_t sim_time; ///< the current sim time in this world in ms GHashTable* superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world - GList* update_list; ///< Models that have a subscriber or controller, and need to be updated + // GList* update_list; ///< Models that have a subscriber or controller, and need to be updated + + GList* reentrant_update_list; ///< It is safe to call these model's Update() in parallel + GList* nonreentrant_update_list; ///< It is NOT safe to call these model's Update() in parallel + long unsigned int updates; ///< the number of simulated time steps executed so far Worldfile* wf; ///< If set, points to the worldfile used to create this world @@ -984,25 +988,13 @@ /** Returns true iff the current time is greater than the time we should quit */ bool PastQuitTime(); + + void StartUpdatingModel( Model* mod ); + void StopUpdatingModel( Model* mod ); - void StartUpdatingModel( Model* mod ) - { - if( ! g_list_find( update_list, mod ) ) - update_list = g_list_append( update_list, mod ); - } + void StartUpdatingModelPose( Model* mod ); + void StopUpdatingModelPose( Model* mod ); - void StopUpdatingModel( Model* mod ) - { update_list = g_list_remove( update_list, mod ); } - - void StartUpdatingModelPose( Model* mod ) - { - if( ! g_list_find( velocity_list, mod ) ) - velocity_list = g_list_append( velocity_list, mod ); - } - - void StopUpdatingModelPose( Model* mod ) - { velocity_list = g_list_remove( velocity_list, mod ); } - static void update_thread_entry( Model* mod, World* world ); public: Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/libstage/world.cc 2009-03-03 18:40:23 UTC (rev 7360) @@ -102,7 +102,9 @@ superregions( g_hash_table_new( (GHashFunc)PointIntHash, (GEqualFunc)PointIntEqual ) ), sr_cached(NULL), - update_list( NULL ), + // update_list( NULL ), + reentrant_update_list( NULL ), + nonreentrant_update_list( NULL ), updates( 0 ), wf( NULL ) { @@ -377,8 +379,11 @@ g_hash_table_remove_all( models_by_name ); - g_list_free( update_list ); - update_list = NULL; + g_list_free( reentrant_update_list ); + reentrant_update_list = NULL; + + g_list_free( nonreentrant_update_list ); + nonreentrant_update_list = NULL; g_list_free( ray_list ); ray_list = NULL; @@ -432,29 +437,27 @@ // something that takes charge LISTMETHOD( charge_list, Model*, UpdateCharge ); - // then update all sensors + // then update all models on the update lists + LISTMETHOD( nonreentrant_update_list, Model*, UpdateIfDue ); + if( worker_threads == 0 ) // do all the work in this thread { - LISTMETHOD( update_list, Model*, UpdateIfDue ); + LISTMETHOD( reentrant_update_list, Model*, UpdateIfDue ); } else // use worker threads { // push the update for every model that needs it into the thread pool - for( GList* it = update_list; it; it=it->next ) + for( GList* it = reentrant_update_list; it; it=it->next ) { Model* mod = (Model*)it->data; if( mod->UpdateDue() ) { - if( mod->thread_safe ) // do update in a worker thread - { - g_mutex_lock( thread_mutex ); - update_jobs_pending++; - g_mutex_unlock( thread_mutex ); - g_thread_pool_push( threadpool, mod, NULL ); - } - else - mod->Update(); // do update in this thread + // printf( "updating model %s in WORKER thread\n", mod->Token() ); + g_mutex_lock( thread_mutex ); + update_jobs_pending++; + g_mutex_unlock( thread_mutex ); + g_thread_pool_push( threadpool, mod, NULL ); } } @@ -953,3 +956,41 @@ { g_hash_table_insert( option_table, (void*)opt->htname, opt ); } + +void World::StartUpdatingModel( Model* mod ) +{ + //if( ! g_list_find( update_list, mod ) ) + // update_list = g_list_append( update_list, mod ); + + if( mod->thread_safe ) + { + if( ! g_list_find( reentrant_update_list, mod ) ) + reentrant_update_list = g_list_append( reentrant_update_list, mod ); + } + else + { + if( ! g_list_find( nonreentrant_update_list, mod ) ) + nonreentrant_update_list = g_list_append( nonreentrant_update_list, mod ); + } +} + +void World::StopUpdatingModel( Model* mod ) +{ + // update_list = g_list_remove( update_list, mod ); + + if( mod->thread_safe ) + reentrant_update_list = g_list_remove( reentrant_update_list, mod ); + else + nonreentrant_update_list = g_list_remove( nonreentrant_update_list, mod ); +} + +void World::StartUpdatingModelPose( Model* mod ) +{ + if( ! g_list_find( velocity_list, mod ) ) + velocity_list = g_list_append( velocity_list, mod ); +} + +void World::StopUpdatingModelPose( Model* mod ) +{ + velocity_list = g_list_remove( velocity_list, mod ); +} Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-03 18:40:23 UTC (rev 7360) @@ -845,11 +845,17 @@ std::set<Option*, Option::optComp> options; std::vector<Option*> modOpts; - for( GList* it=update_list; it; it=it->next ) + for( GList* it=reentrant_update_list; it; it=it->next ) { modOpts = ((Model*)it->data)->getOptions(); options.insert( modOpts.begin(), modOpts.end() ); } + + for( GList* it=nonreentrant_update_list; it; it=it->next ) + { + modOpts = ((Model*)it->data)->getOptions(); + options.insert( modOpts.begin(), modOpts.end() ); + } drawOptions.assign( options.begin(), options.end() ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-02 02:06:19 UTC (rev 7359) +++ code/stage/trunk/worlds/fasr.world 2009-03-03 18:40:23 UTC (rev 7360) @@ -13,8 +13,8 @@ resolution 0.02 # threads may speed things up here depending on available CPU cores & workload - threadpool 0 -# threadpool 3 +# threadpool 0 + threadpool 0 # configure the GUI window @@ -72,7 +72,7 @@ define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) + sicklaser( samples 16 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" joules 100000 joules_capacity 400000 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-03 20:45:39
|
Revision: 7361 http://playerstage.svn.sourceforge.net/playerstage/?rev=7361&view=rev Author: rtv Date: 2009-03-03 20:45:37 +0000 (Tue, 03 Mar 2009) Log Message: ----------- attempt to fix GL bug in laser code (bug 2642532) Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/examples/ctrl/fasr.cc =================================================================== --- code/stage/trunk/examples/ctrl/fasr.cc 2009-03-03 18:40:23 UTC (rev 7360) +++ code/stage/trunk/examples/ctrl/fasr.cc 2009-03-03 20:45:37 UTC (rev 7361) @@ -368,9 +368,6 @@ printf( "unrecognized mode %u\n", robot->mode ); } - //if( robot->charger_ahoy ) - //return 1; - //else return 0; } Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-03-03 18:40:23 UTC (rev 7360) +++ code/stage/trunk/libstage/model_getset.cc 2009-03-03 20:45:37 UTC (rev 7361) @@ -180,19 +180,12 @@ if( parent == NULL ) return pose; - // otherwise - + // otherwise Pose global_pose = pose_sum( parent->GetGlobalPose(), pose ); // we are on top of our parent global_pose.z += parent->geom.size.z; - // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]", - // global_pose.x, - // global_pose.y, - // global_pose.z, - // global_pose.a ); - return global_pose; } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-03-03 18:40:23 UTC (rev 7360) +++ code/stage/trunk/libstage/model_laser.cc 2009-03-03 20:45:37 UTC (rev 7361) @@ -105,9 +105,6 @@ // set default color SetColor( stg_lookup_color(DEFAULT_COLOR)); - if( world->IsGUI() ) - data_dl = glGenLists(1); - RegisterOption( &showLaserData ); RegisterOption( &showLaserStrikes ); RegisterOption( &showLaserFov ); @@ -320,6 +317,9 @@ { data_dirty = false; + if( data_dl < 1 ) + data_dl = glGenLists(1); + glNewList( data_dl, GL_COMPILE ); glTranslatef( 0,0, geom.size.z/2.0 ); // shoot the laser beam out at the right height Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-03-03 18:40:23 UTC (rev 7360) +++ code/stage/trunk/libstage/stage.cc 2009-03-03 20:45:37 UTC (rev 7361) @@ -53,18 +53,7 @@ return init_called; } -double Stg::normalize( double a ) -{ - assert( ! isnan(a) ); - - //return( atan2(sin(a), cos(a))); - // faster than return( atan2(sin(a), cos(a))); - while( a < -M_PI ) a += (2.0 * M_PI); - while( a > M_PI ) a -= (2.0 * M_PI); - return a; -}; - void Stg::RegisterModel( stg_model_type_t type, const char* name, stg_creator_t creator ) @@ -210,21 +199,6 @@ } -// returns the pose of p2 in p1's coordinate system -Pose Stg::pose_sum( const Pose& p1, const Pose& p2 ) -{ - double cosa = cos(p1.a); - double sina = sin(p1.a); - - Pose result; - result.x = p1.x + p2.x * cosa - p2.y * sina; - result.y = p1.y + p2.x * sina + p2.y * cosa; - result.z = p1.z + p2.z; - result.a = normalize(p1.a + p2.a); - - return result; -} - // returns the resultant of vector [p1] and [p2] Pose Stg::pose_scale( const Pose& p1, const double sx, const double sy, const double sz ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-03 18:40:23 UTC (rev 7360) +++ code/stage/trunk/libstage/stage.hh 2009-03-03 20:45:37 UTC (rev 7361) @@ -158,7 +158,12 @@ inline double dtor( double d ){ return( d*M_PI/180.0 ); } /** Normalize an angle to within +/_ M_PI. */ - double normalize( double a ); + inline double normalize( const double a ) + { + static const double TWO_PI = 2.0 * M_PI; + assert( ! isnan(a) ); + return( fmod(a + M_PI, TWO_PI ) - M_PI); + }; /** take binary sign of a, either -1, or 1 if >= 0 */ inline int sgn( int a){ return( a<0 ? -1 : 1); } @@ -562,8 +567,20 @@ stg_color_t stg_lookup_color(const char *name); /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */ - Pose pose_sum( const Pose& p1, const Pose& p2 ); - + inline Pose pose_sum( const Pose& p1, const Pose& p2 ) + { + double cosa = cos(p1.a); + double sina = sin(p1.a); + + Pose result; + result.x = p1.x + p2.x * cosa - p2.y * sina; + result.y = p1.y + p2.x * sina + p2.y * cosa; + result.z = p1.z + p2.z; + result.a = normalize(p1.a + p2.a); + + return result; + } + /** returns a new pose, with each axis scaled */ Pose pose_scale( const Pose& p1, const double x, const double y, const double z ); @@ -735,66 +752,6 @@ record within a model and called whenever the record is set.*/ typedef int (*stg_model_callback_t)( Model* mod, void* user ); - // class Puck -// { -// private: -// void BuildDisplayList(); - -// public: -// stg_color_t color; -// int displaylist; -// stg_meters_t height; -// Pose pose; -// stg_meters_t radius; - -// Puck(); -// void Load( Worldfile* wf, int section ); -// void Save( Worldfile* wf, int section ); - -// void Draw(); -// }; - - - // class EventQueue -// { -// private: -// GTree* future; - -// static gint InstantCompare( stg_usec_t a, stg_usec_t b ) -// { -// if( a < b ) -// return -1; - -// if( a > b ) -// return 1; - -// return 0; // they are equal -// } - -// public: -// EventQueue() -// { -// future = g_tree_new( InstantCompare ); -// } - -// RunInstant() ///< Updates all events due at the next instant -// { -// GList* instant_list = -// } - -// QueueModel( Model* mod ) ///< Adds the model to the event queue -// { -// g_tree_insert( mod->updatedue, g_list_prepend( g_tree_lookup( mod->updatedue ), mod ); ); -// } - -// DeQueueModel( Model* mod ) ///< removes the model from the event queue -// { -// g_tree_insert( mod->updatedue, g_list_remove( g_tree_lookup( mod->updatedue ), mod )); -// } - -// }; - - // ANCESTOR CLASS /** Base class for Model and World */ class Ancestor This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-04 22:19:15
|
Revision: 7362 http://playerstage.svn.sourceforge.net/playerstage/?rev=7362&view=rev Author: rtv Date: 2009-03-04 22:19:10 +0000 (Wed, 04 Mar 2009) Log Message: ----------- fixed issue where powerpack charge changes were not always drawnin th GUI Modified Paths: -------------- code/stage/trunk/libstage/powerpack.cc code/stage/trunk/worlds/fasr.world Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-03 20:45:37 UTC (rev 7361) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-04 22:19:10 UTC (rev 7362) @@ -169,4 +169,6 @@ Subtract( amount ); dest->Add( amount ); + + mod->NeedRedraw(); } Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-03-03 20:45:37 UTC (rev 7361) +++ code/stage/trunk/worlds/fasr.world 2009-03-04 22:19:10 UTC (rev 7362) @@ -154,23 +154,7 @@ puck( pose [ 1.412 3.604 0 0 ] ) autorob( pose [5.418 7.478 0 -163.478] joules 300000 ) -autorob( pose [7.574 6.269 0 -111.715] joules 100000 ) -autorob( pose [5.615 6.185 0 107.666] joules 200000 ) -autorob( pose [7.028 6.502 0 -128.279] joules 400000 ) -autorob( pose [5.750 4.137 0 -97.047] joules 100000 ) -autorob( pose [4.909 6.097 0 -44.366] joules 200000 ) -autorob( pose [6.898 4.775 0 -117.576] joules 300000 ) -autorob( pose [7.394 5.595 0 129.497] joules 400000 ) -autorob( pose [6.468 6.708 0 170.743] joules 100000 ) -autorob( pose [6.451 4.189 0 -61.453] joules 200000 ) -autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) -autorob( pose [4.161 5.544 0 -147.713] joules 400000 ) -autorob( pose [4.911 4.552 0 -125.236] joules 100000 ) -autorob( pose [3.985 6.474 0 -158.025] joules 200000 ) -autorob( pose [5.440 5.317 0 -26.545] joules 300000 ) -autorob( pose [6.362 5.632 0 163.239] joules 400000 ) - #autorob( pose [7.559 4.764 0 -139.066] ) #autorob( pose [5.471 7.446 0 77.301] ) #autorob( pose [7.122 4.175 0 -31.440] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |