Revision: 8588
http://playerstage.svn.sourceforge.net/playerstage/?rev=8588&view=rev
Author: rtv
Date: 2010-03-13 01:09:52 +0000 (Sat, 13 Mar 2010)
Log Message:
-----------
bugfix, fasr2
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/libstage/blockgroup.cc
code/stage/trunk/libstage/model_draw.cc
code/stage/trunk/libstage/powerpack.cc
code/stage/trunk/worlds/fasr2.world
Modified: code/stage/trunk/examples/ctrl/fasr2.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-12 08:06:06 UTC (rev
8587)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-13 01:09:52 UTC (rev
8588)
@@ -77,7 +77,7 @@
void Draw() const
{
- glPointSize(4);
+ glPointSize(3);
FOR_EACH( it, nodes ){ (*it)->Draw(); }
}
@@ -126,7 +126,7 @@
{
public:
Graph& graph;
-
+
GraphVis( Graph& graph )
: Visualizer( "graph", "vis_graph" ), graph(graph) {}
virtual ~GraphVis(){}
@@ -140,7 +140,10 @@
//mod->PushColor( 1,0,0,1 );
- mod->PushColor( mod->GetColor() );
+ Color c = mod->GetColor();
+ c.a = 0.4;
+
+ mod->PushColor( c );
graph.Draw();
mod->PopColor();
@@ -231,10 +234,11 @@
static Model* map_model;
bool fiducial_sub;
+ bool laser_sub;
bool ranger_sub;
-
public:
+
Robot( ModelPosition* pos,
Model* source,
Model* sink,
@@ -263,7 +267,8 @@
last_node( NULL ),
node_interval( 20 ),
node_interval_countdown( node_interval ),
- fiducial_sub(false),
+ fiducial_sub(false),
+ laser_sub(false),
ranger_sub(false)
{
// need at least these models to get any work done
@@ -282,8 +287,8 @@
// LaserUpdate() controls the robot, by reading from laser and
// writing to position
- laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
- laser->Subscribe();
+ //laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ EnableLaser(true);
// we subscribe to the fiducial device only while docking
fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate,
this );
@@ -312,15 +317,6 @@
g.size.x/(float)map_width,
g.size.y/(float)map_height );
- // putchar( '\n' );
- // for( unsigned int y=0; y<map_height; y++ )
- // {
- // for( unsigned int x=0; x<map_width; x++ )
- // printf( "%3d,", map_data[x +
((map_height-y-1)*map_width)] ? 999 : 1 );
-
- // puts("");
- // }
-
// fix the node costs for astar: 0=>1, 1=>9
unsigned int sz = map_width * map_height;
@@ -340,7 +336,9 @@
//( goal );
//puts("");
}
-
+
+
+
void EnableRanger( bool on )
{
if( on && !ranger_sub )
@@ -356,6 +354,21 @@
}
}
+ void EnableLaser( bool on )
+ {
+ if( on && !laser_sub )
+ {
+ laser_sub = true;
+ laser->Subscribe();
+ }
+
+ if( !on && laser_sub )
+ {
+ laser_sub = false;
+ laser->Unsubscribe();
+ }
+ }
+
void EnableFiducial( bool on )
{
if( on && !fiducial_sub )
@@ -371,11 +384,13 @@
}
}
-
- void Plan( Model* dest )
+ void Plan( Pose sp )
{
+ // change my color to that of my destination
+ //pos->SetColor( dest->GetColor() );
+
Pose pose = pos->GetPose();
- Pose sp = dest->GetPose();
+ //Pose sp = dest->GetPose();
Geom g = map_model->GetGeom();
point_t start( MetersToCell(pose.x, g.size.x, map_width),
@@ -432,24 +447,14 @@
if( charger_ahoy )
{
+ // drive toward the charger
double a_goal = normalize( charger_bearing );
+ // helps to align with the way the charger is pointing
double orient = normalize( M_PI - (charger_bearing -
charger_heading) );
- //printf( "val %.2f\n", orient );
-
- //if( fabs(orient) < M_PI/4.0 )
+
a_goal = normalize( a_goal - 2.0 * orient );
-
-
- // if( pos->Stalled() )
- // {
- // puts( "stalled. stopping" );
- // pos->Stop();
- // }
- // else
-
- // a_goal *= 2.0;
-
+
if( charger_range > creep_distance )
{
if( !ObstacleAvoid() )
@@ -458,7 +463,7 @@
pos->SetTurnSpeed( a_goal );
}
}
- else
+ else // we are very close!
{
pos->SetTurnSpeed( a_goal );
pos->SetXSpeed( 0.02 ); // creep towards it
@@ -467,6 +472,7 @@
{
pos->Stop();
docked_angle = pos->GetPose().a;
+ EnableLaser( false );
}
if( pos->Stalled() ) // touching
@@ -487,7 +493,7 @@
//printf( "fully charged, now back to work\n" );
mode = MODE_UNDOCK;
EnableRanger(true); // enable the sonar to see behind us
- //EnableFiducial(false);
+ EnableLaser(true);
}
}
@@ -623,11 +629,12 @@
void SetGoal( Model* goal )
- {
+ {
if( goal != this->goal )
{
this->goal = goal;
- Plan( goal );
+ Plan( goal->GetPose() );
+ pos->SetColor( goal->GetColor() );
}
}
@@ -641,8 +648,6 @@
Pose pose = pos->GetPose();
-
-
double a_goal = 0;
// dtor( ( pos->GetFlagCount() ) ? have[y][x] :
need[y][x] ); // map
@@ -666,8 +671,8 @@
// goal->Token(),
// goal->GetPose().x,
// goal->GetPose().y );
- Plan( goal );
- cached_goal_pose = goal->GetPose();
+ Plan( goal->GetPose() );
+ cached_goal_pose = goal->GetPose();
}
// if we are low on juice - find the direction to the
recharger instead
@@ -711,42 +716,11 @@
}
- // 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;
+// // inspect the laser data and decide what to do
+// static int LaserUpdate( ModelLaser* laser, Robot* robot )
+// {
+// }
- assert( laser->GetSamples().size() > 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 );
- }
-
- return 0;
- }
-
bool Hungry()
{
return( pos->FindPowerPack()->ProportionRemaining() < 0.2 );
@@ -819,7 +793,38 @@
//printf( "diss: %.2f\n", pos->FindPowerPack()->GetDissipated() );
+
+ // 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;
+
+ //assert( robot->laser->GetSamples().size() > 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 );
+ }
+
+
return 0; // run again
}
@@ -876,8 +881,8 @@
// STATIC VARS
pthread_mutex_t Robot::planner_mutex = PTHREAD_MUTEX_INITIALIZER;
-unsigned int Robot::map_width( 60 );
-unsigned int Robot::map_height( 60 );
+unsigned int Robot::map_width( 50 );
+unsigned int Robot::map_height( 50 );
uint8_t* Robot::map_data( NULL );
Model* Robot::map_model( NULL );
@@ -895,6 +900,9 @@
}
}
+const std::string sources[] = {"red", "green", "blue", "cyan", "yellow",
"magenta" };
+const unsigned int sources_count = 6;
+
// Stage calls this when the model starts up
extern "C" int Init( Model* mod, CtrlArgs* args )
{
@@ -912,10 +920,15 @@
// expecting a task color name as the 1th argument
assert( words.size() == 2 );
assert( words[1].size() > 0 );
+
+ // pick a source at random
+ const std::string& color = sources[ rand() % sources_count ];
new Robot( (ModelPosition*)mod,
- mod->GetWorld()->GetModel( words[1] +
"_source" ),
- mod->GetWorld()->GetModel( words[1] + "_sink"
),
+ //mod->GetWorld()->GetModel( words[1] +
"_source" ),
+ //mod->GetWorld()->GetModel( words[1] +
"_sink" ),
+ mod->GetWorld()->GetModel( color + "_source" ),
+ mod->GetWorld()->GetModel( color + "_sink" ),
mod->GetWorld()->GetModel( "fuel_zone" ) );
return 0; //ok
Modified: code/stage/trunk/libstage/blockgroup.cc
===================================================================
--- code/stage/trunk/libstage/blockgroup.cc 2010-03-12 08:06:06 UTC (rev
8587)
+++ code/stage/trunk/libstage/blockgroup.cc 2010-03-13 01:09:52 UTC (rev
8588)
@@ -150,6 +150,8 @@
void BlockGroup::BuildDisplayList( Model* mod )
{
+ //puts( "build" );
+
if( ! mod->world->IsGUI() )
return;
@@ -237,8 +239,11 @@
void BlockGroup::CallDisplayList( Model* mod )
{
- if( displaylist == 0 )
+ if( displaylist == 0 || mod->rebuild_displaylist )
+ {
BuildDisplayList( mod );
+ mod->rebuild_displaylist = 0;
+ }
glCallList( displaylist );
}
Modified: code/stage/trunk/libstage/model_draw.cc
===================================================================
--- code/stage/trunk/libstage/model_draw.cc 2010-03-12 08:06:06 UTC (rev
8587)
+++ code/stage/trunk/libstage/model_draw.cc 2010-03-13 01:09:52 UTC (rev
8588)
@@ -308,8 +308,8 @@
glRotatef( robotAngle - yaw, 0,0,1 );
glRotatef( -pitch, 1,0,0 );
- if( power_pack )
- power_pack->Visualize( cam );
+ // if( power_pack )
+ //power_pack->Visualize( cam );
if( !say_string.empty() )
{
Modified: code/stage/trunk/libstage/powerpack.cc
===================================================================
--- code/stage/trunk/libstage/powerpack.cc 2010-03-12 08:06:06 UTC (rev
8587)
+++ code/stage/trunk/libstage/powerpack.cc 2010-03-13 01:09:52 UTC (rev
8588)
@@ -16,7 +16,11 @@
stg_joules_t PowerPack::global_dissipated = 0.0;
PowerPack::PowerPack( Model* mod ) :
- event_vis( 32,32,1.0 ),
+ event_vis( 2.0 * std::max( fabs(ceil(mod->GetWorld()->GetExtent().x.max)),
+
fabs(floor(mod->GetWorld()->GetExtent().x.min))),
+ 2.0 * std::max(
fabs(ceil(mod->GetWorld()->GetExtent().y.max)),
+
fabs(floor(mod->GetWorld()->GetExtent().y.min))),
+ 1.0 ),
output_vis( 0,100,200,40, 1200, Color(1,0,0), Color(0,0,0,0.5), "energy
output", "energy_input" ),
stored_vis( 0,142,200,40, 1200, Color(0,1,0), Color(0,0,0,0.5), "energy
stored", "energy_stored" ),
mod( mod),
@@ -32,8 +36,8 @@
mod->world->AddPowerPack( this );
mod->AddVisualizer( &event_vis, false );
- mod->AddVisualizer( &output_vis, true );
- mod->AddVisualizer( &stored_vis, true );
+ mod->AddVisualizer( &output_vis, false );
+ mod->AddVisualizer( &stored_vis, false );
}
PowerPack::~PowerPack()
Modified: code/stage/trunk/worlds/fasr2.world
===================================================================
--- code/stage/trunk/worlds/fasr2.world 2010-03-12 08:06:06 UTC (rev 8587)
+++ code/stage/trunk/worlds/fasr2.world 2010-03-13 01:09:52 UTC (rev 8588)
@@ -16,16 +16,16 @@
resolution 0.02
-threads 8
+threads 0
# configure the GUI window
window
(
size [ 1226.000 1080.000 ]
- center [ -40.702 -47.522 ]
+ center [ -36.643 -41.795 ]
rotate [ 0 0 ]
- scale 22.245
+ scale 41.558
show_data 1
show_flags 1
@@ -147,16 +147,16 @@
-charge_station( pose [ -38.500 -49.9 0 90.000 ] )
-charge_station( pose [ -39.500 -49.9 0 90.000 ] )
-charge_station( pose [ -40.500 -49.9 0 90.000 ] )
-charge_station( pose [ -41.500 -49.9 0 90.000 ] )
-charge_station( pose [ -42.500 -49.9 0 90.000 ] )
-charge_station( pose [ -43.500 -49.9 0 90.000 ] )
-charge_station( pose [ -44.500 -49.9 0 90.000 ] )
-charge_station( pose [ -45.500 -49.9 0 90.000 ] )
-charge_station( pose [ -46.500 -49.9 0 90.000 ] )
-charge_station( pose [ -47.500 -49.9 0 90.000 ] )
+charge_station( pose [ -38.500 -49.900 0 90.000 ] )
+charge_station( pose [ -39.500 -49.900 0 90.000 ] )
+charge_station( pose [ -40.500 -49.900 0 90.000 ] )
+charge_station( pose [ -41.500 -49.900 0 90.000 ] )
+charge_station( pose [ -42.500 -49.900 0 90.000 ] )
+charge_station( pose [ -43.500 -49.900 0 90.000 ] )
+charge_station( pose [ -44.500 -49.900 0 90.000 ] )
+charge_station( pose [ -45.500 -49.900 0 90.000 ] )
+charge_station( pose [ -46.500 -49.900 0 90.000 ] )
+charge_station( pose [ -47.500 -49.900 0 90.000 ] )
#charge_station( pose [ -38.00 -49.9 0 90.000 ] )
#charge_station( pose [ -39.00 -49.9 0 90.000 ] )
@@ -169,16 +169,16 @@
#charge_station( pose [ -46.00 -49.9 0 90.000 ] )
#charge_station( pose [ -47.0 -49.9 0 90.000 ] )
-charge_station( pose [ -49.9 -38.500 0 0 ] )
-charge_station( pose [ -49.9 -39.500 0 0 ] )
-charge_station( pose [ -49.9 -40.500 0 0 ] )
-charge_station( pose [ -49.9 -41.500 0 0 ] )
-charge_station( pose [ -49.9 -42.500 0 0 ] )
-charge_station( pose [ -49.9 -43.500 0 0 ] )
-charge_station( pose [ -49.9 -44.500 0 0 ] )
-charge_station( pose [ -49.9 -45.500 0 0 ] )
-charge_station( pose [ -49.9 -46.500 0 0 ] )
-charge_station( pose [ -49.9 -47.500 0 0 ] )
+charge_station( pose [ -49.900 -38.500 0 0 ] )
+charge_station( pose [ -49.900 -39.500 0 0 ] )
+charge_station( pose [ -49.900 -40.500 0 0 ] )
+charge_station( pose [ -49.900 -41.500 0 0 ] )
+charge_station( pose [ -49.900 -42.500 0 0 ] )
+charge_station( pose [ -49.900 -43.500 0 0 ] )
+charge_station( pose [ -49.900 -44.500 0 0 ] )
+charge_station( pose [ -49.900 -45.500 0 0 ] )
+charge_station( pose [ -49.900 -46.500 0 0 ] )
+charge_station( pose [ -49.900 -47.500 0 0 ] )
zone( color "orange"
pose [ -43.000 -43.000 0 0 ]
@@ -332,7 +332,7 @@
#cyanbot( pose [-6.301 18.782 0 -117.456] kjoules 400 )
greenbot( pose [-35.549 -47.514 0 -41.980] kjoules 40 )
-greenbot( pose [-48.064 -48.105 0 -119.991] kjoules 40 )
+greenbot( pose [-45.547 -46.397 0 -119.991] kjoules 40 )
greenbot( pose [-38.902 -42.142 0 -65.139] kjoules 40 )
greenbot( pose [-39.615 -42.398 0 -73.426] kjoules 40 )
greenbot( pose [-39.830 -40.428 0 -93.488] kjoules 40 )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
