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. ------------------------------------------------------------------------------ Create and Deploy Rich Internet Apps outside the browser with Adobe(R)AIR(TM) software. With Adobe AIR, Ajax developers can use existing skills and code to build responsive, highly engaging applications that combine the power of local resources and data with the reach of the web. Download the Adobe AIR SDK and Ajax docs to start building applications today-http://p.sf.net/sfu/adobe-com _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit