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
