Revision: 8589
http://playerstage.svn.sourceforge.net/playerstage/?rev=8589&view=rev
Author: rtv
Date: 2010-03-15 17:54:04 +0000 (Mon, 15 Mar 2010)
Log Message:
-----------
fasr2, and threading
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/examples/ctrl/source.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/powerpack.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/fasr2.world
Modified: code/stage/trunk/examples/ctrl/fasr2.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-13 01:09:52 UTC (rev
8588)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-15 17:54:04 UTC (rev
8589)
@@ -19,12 +19,12 @@
typedef enum {
MODE_WORK=0,
MODE_DOCK,
- MODE_UNDOCK
+ MODE_UNDOCK,
+ MODE_QUEUE
} nav_mode_t;
-
class Edge;
class Node
@@ -34,7 +34,7 @@
double value;
std::vector<Edge*> edges;
- Node( const Pose& pose )
+ Node( Pose pose )
: pose(pose), value(0), edges() {}
~Node();
@@ -58,6 +58,7 @@
: to(to), cost(cost) {}
};
+
class Graph
{
public:
@@ -109,7 +110,7 @@
if( best_node == NULL )
{
- puts( "warning: no nodes in range" );
+ printf( "FASR warning: no nodes in range" );
return false;
}
@@ -282,20 +283,21 @@
// pos->GetUnusedModelOfType( "laser" );
// PositionUpdate() checks to see if we reached source or sink
- pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+ pos->AddUpdateCallback( (stg_model_callback_t)UpdateCallback, this );
pos->Subscribe();
+
+
+ // no other callbacks - we access all other data from the position
+ // callback
- // LaserUpdate() controls the robot, by reading from laser and
- // writing to position
- //laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ // start with only the laser enabled - we switch laser, ranger and
+ // fiducial on and off as needed to save energy and compute time
+
+ //laser->AddUpdateCallback(
(stg_model_callback_t)Robot::LaserUpdateCallback, this );
+
+
EnableLaser(true);
- // we subscribe to the fiducial device only while docking
- fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate,
this );
-
- // we subscribe to the ranger device only while undocking (i.e.
backing up)
- //ranger->AddUpdateCallback( (stg_model_callback_t)RangerUpdate, this
);
-
pos->AddVisualizer( &graphvis, true );
if( map_data == NULL )
@@ -327,7 +329,7 @@
else if( map_data[i] == 1 )
map_data[i] = 9;
else
- printf( "bad value %d in map at index %d\n",
(int)map_data[i], (int)i );
+ printf( "FASR: bad value %d in map at index
%d\n", (int)map_data[i], (int)i );
assert( (map_data[i] == 1) || (map_data[i] ==
9) );
}
@@ -413,9 +415,11 @@
path );
pthread_mutex_unlock( &planner_mutex );
+
+ if( ! result )
+ printf( "FASR warning: plan failed to find path from
(%.2f,%.2f) to (%.2f,%.2f)\n",
+ pose.x, pose.y, sp.x, sp.y );
- //printf( "#%s:\n", result ? "PATH" : "NOPATH" );
-
graph.nodes.clear();
unsigned int dist = 0;
@@ -481,7 +485,7 @@
}
else
{
- printf( "%s docking but can't see a charger\n", pos->Token()
);
+ printf( "FASR: %s docking but can't see a charger\n",
pos->Token() );
pos->Stop();
EnableFiducial( false );
mode = MODE_WORK; // should get us back on track eventually
@@ -507,7 +511,10 @@
const unsigned int BACK_SENSOR_FIRST = 10;
const unsigned int BACK_SENSOR_LAST = 13;
-
+ // make sure the required sensors are running
+ assert( ranger_sub );
+ assert( fiducial_sub );
+
if( charger_range < back_off_distance )
{
pos->SetXSpeed( back_off_speed );
@@ -660,7 +667,7 @@
if( Hungry() )
SetGoal( fuel_zone );
- // if the graph failes to offer advice or the goal has moved a
+ // if the graph fails to offer advice or the goal has moved a
// ways since last time we planned
if( (graph.GoodDirection( pose, 4.0, a_goal ) == 0) ||
(goal->GetPose().Distance2D( cached_goal_pose )
> 2.0) )
@@ -708,19 +715,7 @@
}
}
-
- static int RangerUpdate( ModelRanger* ranger, Robot* robot )
- {
- //printf( "%s RANGER UPDATE", ranger->Token() );
- return 0;
- }
-
-// // inspect the laser data and decide what to do
-// static int LaserUpdate( ModelLaser* laser, Robot* robot )
-// {
-// }
-
bool Hungry()
{
return( pos->FindPowerPack()->ProportionRemaining() < 0.2 );
@@ -730,68 +725,119 @@
{
return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
}
+
+ // static callback wrapper
+ static int UpdateCallback( ModelLaser* laser, Robot* robot )
+ {
+ return robot->Update();
+ }
- static int PositionUpdate( ModelPosition* pos, Robot* robot )
- {
+ int Update( void )
+ {
Pose pose = pos->GetPose();
#if 0
// when countdown reaches zero
- if( --robot->node_interval_countdown == 0 )
+ if( --node_interval_countdown == 0 )
{
// reset countdown
- robot->node_interval_countdown = robot->node_interval;
+ node_interval_countdown = node_interval;
Node* node = new Node( pose );
- robot->graph.AddNode( node );
+ graph.AddNode( node );
- if( robot->last_node )
- robot->last_node->AddEdge( new Edge( node ) );
+ if( last_node )
+ last_node->AddEdge( new Edge( node ) );
- robot->last_node = node;
+ last_node = node;
// limit the number of nodes
- while( robot->graph.nodes.size() > TRAIL_LENGTH_MAX )
- robot->graph.PopFront();
+ while( graph.nodes.size() > TRAIL_LENGTH_MAX )
+ graph.PopFront();
}
#endif
+
+ // assume we can't see the charger
+ charger_ahoy = false;
+
+ // if the fiducial is enabled
+ if( fiducial_sub )
+ {
+ std::vector<ModelFiducial::Fiducial>& fids =
fiducial->GetFiducials();
+
+ if( fids.size() > 0 )
+ {
+ ModelFiducial::Fiducial* closest = NULL;
+
+ for( unsigned int i = 0; i < fids.size(); i++ )
+ {
+ //printf( "fiducial %d is %d at %.2f m
%.2f radians\n",
+ // i, f->id, f->range,
f->bearing );
+
+ ModelFiducial::Fiducial* f = &fids[i];
+
+ // find the closest
+ if( f->id == 2 && ( closest == NULL ||
f->range < closest->range )) // I see a charging station
+ closest = f;
+ }
+
+ if( closest )
+ { // record that I've
seen it and where it is
+ charger_ahoy = true;
+
+ //printf( "AHOY %s\n", pos->Token() );
+
+ charger_bearing = closest->bearing;
+ charger_range = closest->range;
+ charger_heading = closest->geom.a;
+ }
+ }
+ }
+
+
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
// pose.x, pose.y, pose.z, pose.a );
//pose.z += 0.0001;
- //robot->pos->SetPose( pose );
+ //pos->SetPose( pose );
// if we're close to the source we get a flag
- Pose sourcepose = robot->source->GetPose();
- Geom sourcegeom = robot->source->GetGeom();
+ Pose sourcepose = source->GetPose();
+ Geom sourcegeom = source->GetGeom();
- if( hypot( sourcepose.x-pose.x, sourcepose.y-pose.y ) <
sourcegeom.size.x/2.0 &&
+ stg_meters_t dest_dist = hypot( sourcepose.x-pose.x,
sourcepose.y-pose.y );
+
+ // if we're close, go get in line
+ // if( dest_dist < sourcegeom.size.x )
+ // mode = MODE_QUEUE;
+
+
+ if( dest_dist < sourcegeom.size.x/2.0 &&
pos->GetFlagCount() < PAYLOAD )
{
// transfer a chunk from source to robot
- pos->PushFlag( robot->source->PopFlag() );
+ pos->PushFlag( source->PopFlag() );
if( pos->GetFlagCount() == PAYLOAD )
- robot->SetGoal( robot->sink ); // we're done working
+ SetGoal( sink ); // we're done working
}
// if we're close to the sink we lose a flag
- Pose sinkpose = robot->sink->GetPose();
- Geom sinkgeom = robot->sink->GetGeom();
+ Pose sinkpose = sink->GetPose();
+ Geom sinkgeom = sink->GetGeom();
if( hypot( sinkpose.x-pose.x, sinkpose.y-pose.y ) <
sinkgeom.size.x/2.0 &&
pos->GetFlagCount() > 0 )
{
//puts( "dropping" );
// transfer a chunk between robot and goal
- robot->sink->PushFlag( pos->PopFlag() );
+ sink->PushFlag( pos->PopFlag() );
if( pos->GetFlagCount() == 0 )
- robot->SetGoal( robot->source ); // we're done
dropping off
+ SetGoal( source ); // we're done dropping off
}
-
-
+
//printf( "diss: %.2f\n", pos->FindPowerPack()->GetDissipated() );
// if( laser->power_pack && laser->power_pack->charging )
@@ -801,27 +847,30 @@
//if( laser->GetSamples(NULL) == NULL )
//return 0;
- //assert( robot->laser->GetSamples().size() > 0 );
+ //assert( laser->GetSamples().size() > 0 );
- switch( robot->mode )
+ switch( mode )
{
case MODE_DOCK:
//puts( "DOCK" );
- robot->Dock();
+ Dock();
break;
case MODE_WORK:
//puts( "WORK" );
- robot->Work();
+ Work();
break;
case MODE_UNDOCK:
//puts( "UNDOCK" );
- robot->UnDock();
+ UnDock();
break;
+ //case MODE_QUEUE:
+ //Queue();
+
default:
- printf( "unrecognized mode %u\n", robot->mode );
+ printf( "unrecognized mode %u\n", mode );
}
@@ -829,40 +878,13 @@
}
-
- static int FiducialUpdate( ModelFiducial* mod, Robot* robot )
- {
- robot->charger_ahoy = false;
+ void Queue()
+ {
- std::vector<ModelFiducial::Fiducial>& fids = mod->GetFiducials();
-
- ModelFiducial::Fiducial* closest = NULL;
-
- for( unsigned int i = 0; i < fids.size(); i++ )
- {
- //printf( "fiducial %d is %d at %.2f m %.2f radians\n",
- // i, f->id, f->range, f->bearing );
-
- ModelFiducial::Fiducial* f = &fids[i];
-
- // find the closest
- if( f->id == 2 && ( closest == NULL || f->range <
closest->range )) // I see a charging station
- closest = f;
- }
-
- if( closest )
- { // record that I've seen it and where
it is
- robot->charger_ahoy = true;
-
- //printf( "AHOY %s\n", robot->pos->Token() );
- robot->charger_bearing = closest->bearing;
- robot->charger_range = closest->range;
- robot->charger_heading = closest->geom.a;
- }
-
- return 0; // run again
}
+
+
static int FlagIncr( Model* mod, Robot* robot )
@@ -925,8 +947,6 @@
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( color + "_source" ),
mod->GetWorld()->GetModel( color + "_sink" ),
mod->GetWorld()->GetModel( "fuel_zone" ) );
Modified: code/stage/trunk/examples/ctrl/source.cc
===================================================================
--- code/stage/trunk/examples/ctrl/source.cc 2010-03-13 01:09:52 UTC (rev
8588)
+++ code/stage/trunk/examples/ctrl/source.cc 2010-03-15 17:54:04 UTC (rev
8589)
@@ -5,18 +5,7 @@
const double FLAGSZ = 0.25;
const unsigned int CAPACITY = 1;
-int Update( Model* mod, void* dummy );
-
-// Stage calls this when the model starts up
-extern "C" int Init( Model* mod )
-{
- mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
- mod->Subscribe();
- return 0; //ok
-}
-
-// inspect the laser data and decide what to do
int Update( Model* mod, void* dummy )
{
if(( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
@@ -26,3 +15,11 @@
return 0; // run again
}
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod )
+{
+ mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
+ mod->Subscribe();
+ return 0; //ok
+}
+
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2010-03-13 01:09:52 UTC (rev 8588)
+++ code/stage/trunk/libstage/model.cc 2010-03-15 17:54:04 UTC (rev 8589)
@@ -734,9 +734,9 @@
void Model::Update( void )
{
- if( hooks.attached_update )
- CallCallbacks( &hooks.update );
-
+ if( hooks.attached_update )
+ CallCallbacks( &hooks.update );
+
last_update = world->sim_time;
world->Enqueue( event_queue_num, interval, this );
}
Modified: code/stage/trunk/libstage/powerpack.cc
===================================================================
--- code/stage/trunk/libstage/powerpack.cc 2010-03-13 01:09:52 UTC (rev
8588)
+++ code/stage/trunk/libstage/powerpack.cc 2010-03-15 17:54:04 UTC (rev
8589)
@@ -48,12 +48,6 @@
mod->RemoveVisualizer( &stored_vis );
}
-
-void PowerPack::Print( char* prefix ) const
-{
- printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity );
-}
-
/** OpenGL visualization of the powerpack state */
void PowerPack::Visualize( Camera* cam )
{
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2010-03-13 01:09:52 UTC (rev 8588)
+++ code/stage/trunk/libstage/stage.hh 2010-03-15 17:54:04 UTC (rev 8589)
@@ -349,14 +349,15 @@
};
- /** Specify a 3 axis velocity in x, y and heading. */
+ /** Specify a 4 axis velocity: 3D vector in [x, y, z], plus rotation
+ about Z (yaw).*/
class Velocity : public Pose
{
public:
- /** @param x x position in meters
- @param y y position in meters
- @param z z position in meters
- @param a heading in radians
+ /** @param x velocity vector component along X axis (forward speed),
in meters per second.
+ @param y velocity vector component along Y axis (sideways
speed), in meters per second.
+ @param z velocity vector component along Z axis (vertical
speed), in meters per second.
+ @param a rotational velocity around Z axis (yaw), in radians
per second.
*/
Velocity( stg_meters_t x,
stg_meters_t y,
@@ -368,13 +369,18 @@
Velocity()
{ /*empty*/ }
- /** Print velocity in human-readable format on stdout
- @param prefix Character string to prepend to pose output
+ /** Print velocity in human-readable format on stdout, with a
+ prefix string
+
+ @param prefix Character string to prepend to output, or NULL.
*/
- virtual void Print( const char* prefix )
+ virtual void Print( const char* prefix ) const
{
- printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
- prefix, x,y,z,a );
+ if( prefix )
+ printf( "%s", prefix );
+
+ printf( "velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n",
+ x,y,z,a );
}
};
@@ -386,10 +392,17 @@
Pose pose;///< position
Size size;///< extent
- void Print( const char* prefix )
+ /** Print geometry in human-readable format on stdout, with a
+ prefix string
+
+ @param prefix Character string to prepend to output, or NULL.
+ */
+ void Print( const char* prefix ) const
{
- printf( "%s geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
- prefix,
+ if( prefix )
+ printf( "%s", prefix );
+
+ printf( "geom pose: (%.2f,%.2f,%.2f) size: [%.2f,%.2f]\n",
pose.x,
pose.y,
pose.a,
@@ -1577,10 +1590,6 @@
/** 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 ) const;
-
/** Returns the energy capacity minus the current amount stored */
stg_joules_t RemainingCapacity() const;
@@ -1596,8 +1605,15 @@
double ProportionRemaining() const
{ return( stored / capacity ); }
+ /** Print human-readable status on stdout, prefixed with the
+ argument string, or NULL */
void Print( const char* prefix ) const
- { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); }
+ {
+ if( prefix )
+ printf( "%s", prefix );
+
+ printf( "PowerPack %.2f/%.2f J\n", stored, capacity );
+ }
stg_joules_t GetStored() const;
stg_joules_t GetCapacity() const;
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2010-03-13 01:09:52 UTC (rev 8588)
+++ code/stage/trunk/libstage/world.cc 2010-03-15 17:54:04 UTC (rev 8589)
@@ -539,6 +539,13 @@
bool World::Update()
{
+ if( show_clock && ((this->updates % show_clock_interval) == 0) )
+ {
+ printf( "\r[Stage: %s]", ClockString().c_str() );
+ fflush( stdout );
+ }
+
+
//puts( "World::Update()" );
// if we've run long enough, exit
@@ -586,12 +593,6 @@
FOR_EACH( it, active_energy )
(*it)->UpdateCharge();
- if( show_clock && ((this->updates % show_clock_interval) == 0) )
- {
- printf( "\r[Stage: %s]", ClockString().c_str() );
- fflush( stdout );
- }
-
++updates;
return false;
Modified: code/stage/trunk/worlds/fasr2.world
===================================================================
--- code/stage/trunk/worlds/fasr2.world 2010-03-13 01:09:52 UTC (rev 8588)
+++ code/stage/trunk/worlds/fasr2.world 2010-03-15 17:54:04 UTC (rev 8589)
@@ -16,7 +16,7 @@
resolution 0.02
-threads 0
+threads 8
# configure the GUI window
window
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
