Update of /cvsroot/playerstage/code/stage/examples/ctrl
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15242/examples/ctrl
Modified Files:
Makefile.am wander.cc
Added Files:
fasr.cc sink.cc source.cc
Log Message:
resource transport demo
Index: wander.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/examples/ctrl/wander.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** wander.cc 22 Feb 2008 21:20:47 -0000 1.1
--- wander.cc 3 Mar 2008 06:02:25 -0000 1.2
***************
*** 12,15 ****
--- 12,16 ----
StgModelPosition* pos;
StgModelLaser* laser;
+ StgModelBlobfinder* blobfinder;
int avoidcount, randcount;
} robot_t;
***************
*** 24,36 ****
robot->pos = (StgModelPosition*)mod;
- robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
- robot->avoidcount = 0;
- robot->randcount = 0;
assert( robot->laser );
robot->laser->Subscribe();
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
//robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate,
robot );
return 0; //ok
}
--- 25,47 ----
robot->pos = (StgModelPosition*)mod;
+ robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
assert( robot->laser );
robot->laser->Subscribe();
+
+ robot->blobfinder = (StgModelBlobfinder*)mod->GetModel( "blobfinder:0" );
+ assert( robot->blobfinder );
+ // robot->blobfinder->Subscribe();
+
+ robot->avoidcount = 0;
+ robot->randcount = 0;
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
//robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate,
robot );
+
+ robot->pos->AddFlag( new StgFlag( (stg_color_t)0xFFAA00AA, 1.0));
+ robot->pos->AddFlag( new StgFlag( (stg_color_t)0xFF0000AA, 0.5));
+ robot->pos->AddFlag( new StgFlag( (stg_color_t)0xFFAA0000, 0.2));
+ robot->pos->AddFlag( new StgFlag( (stg_color_t)0xFF00FFFF, 0.1));
return 0; //ok
}
--- NEW FILE: sink.cc ---
#include "stage.hh"
using namespace Stg;
const int INTERVAL = 100;
int Update( StgModel* mod, void* dummy );
// Stage calls this when the model starts up
extern "C" int Init( StgModel* mod )
{
for( int i=0; i<5; i++ )
mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), 0.5 ) );
mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
return 0; //ok
}
// inspect the laser data and decide what to do
int Update( StgModel* mod, void* dummy )
{
if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
mod->PopFlag();
return 0; // run again
}
--- NEW FILE: fasr.cc ---
#include "stage.hh"
using namespace Stg;
const double cruisespeed = 0.4;
const double avoidspeed = 0.05;
const double avoidturn = 0.5;
const double minfrontdistance = 0.7;
const double stopdist = 0.5;
const bool verbose = false;
const int avoidduration = 10;
double have[4][4] = {
{ 90, 180, 180, 180 },
{ 90, -90, 0, -90 },
{ 90, 90, 180, 90 },
{ 0, 0, 0, 45}
};
double need[4][4] = {
{ -120, -180, 180, 180 },
{ -90, -90, 180, 180 },
{ -90, -90, 180, 180 },
{ -90, -180, -90, -90 }
};
typedef struct
{
StgModelPosition* pos;
StgModelLaser* laser;
StgModelBlobfinder* blobfinder;
StgModel *source, *sink;
int avoidcount, randcount;
int latch;
} robot_t;
int LaserUpdate( StgModel* mod, robot_t* robot );
int PositionUpdate( StgModel* mod, robot_t* robot );
// Stage calls this when the model starts up
extern "C" int Init( StgModel* mod )
{
robot_t* robot = new robot_t;
robot->latch = 0;
//robot->flag = new StgFlag( stg_color_pack( 1,1,0,0.5 ), 3 );
robot->pos = (StgModelPosition*)mod;
robot->laser = (StgModelLaser*)mod->GetModel( "laser:0" );
assert( robot->laser );
robot->laser->Subscribe();
robot->avoidcount = 0;
robot->randcount = 0;
robot->laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, robot );
robot->pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, robot );
robot->source = mod->GetWorld()->GetModel( "source" );
assert(robot->source);
robot->sink = mod->GetWorld()->GetModel( "sink" );
assert(robot->sink);
return 0; //ok
}
// inspect the laser data and decide what to do
int LaserUpdate( StgModel* mod, robot_t* robot )
{
// get the data
uint32_t sample_count=0;
stg_laser_sample_t* scan = robot->laser->GetSamples( &sample_count );
assert(scan);
double newturnrate=0.0, newspeed=0.0;
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;
for (uint32_t i = 0; i < sample_count; i++)
{
if( (i > (sample_count/4))
&& (i < (sample_count - (sample_count/4)))
&& scan[i].range < minfrontdistance)
obstruction = true;
if( scan[i].range < stopdist )
stop = true;
if( i > sample_count/2 )
minleft = MIN( minleft, scan[i].range );
else
minright = MIN( minright, scan[i].range );
}
if( obstruction || stop || (robot->avoidcount>0) )
{
if( verbose ) puts( "Avoid" );
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( verbose ) puts( "Avoid START" );
robot->avoidcount = random() % avoidduration + avoidduration;
if( minleft < minright )
robot->pos->SetTurnSpeed( -avoidturn );
else
robot->pos->SetTurnSpeed( +avoidturn );
}
robot->avoidcount--;
}
else
{
if( verbose ) puts( "Cruise" );
robot->avoidcount = 0;
robot->pos->SetXSpeed( cruisespeed );
stg_pose_t pose = robot->pos->GetPose();
int x = (pose.x + 8) / 4;
int y = (pose.y + 8) / 4;
double a_goal =
dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] );
double a_error = normalize( a_goal - pose.a );
robot->pos->SetTurnSpeed( a_error );
}
return 0;
}
int PositionUpdate( StgModel* mod, robot_t* robot )
{
stg_pose_t pose = robot->pos->GetPose();
if( --robot->latch < 1 )
{
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
// pose.x, pose.y, pose.z, pose.a );
if( hypot( -7-pose.x, -7-pose.y ) < 2.0 )
{
//puts( "collecting" );
robot->pos->PushFlag( robot->source->PopFlag() );
}
if( hypot( 7-pose.x, 7-pose.y ) < 1.0 )
{
//puts( "dropping" );
// transfer a chunk between robot and goal
robot->sink->PushFlag( robot->pos->PopFlag() );
}
robot->latch = 20;
}
return 0; // run again
}
--- NEW FILE: source.cc ---
#include "stage.hh"
using namespace Stg;
const int INTERVAL = 100;
int Update( StgModel* mod, void* dummy );
// Stage calls this when the model starts up
extern "C" int Init( StgModel* mod )
{
for( int i=0; i<5; i++ )
mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0 ), 0.5 ) );
mod->AddUpdateCallback( (stg_model_callback_t)Update, NULL );
return 0; //ok
}
// inspect the laser data and decide what to do
int Update( StgModel* mod, void* dummy )
{
if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), 0.5 ) );
return 0; // run again
}
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/stage/examples/ctrl/Makefile.am,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** Makefile.am 22 Feb 2008 21:20:47 -0000 1.1
--- Makefile.am 3 Mar 2008 06:02:25 -0000 1.2
***************
*** 5,9 ****
! lib_LTLIBRARIES = wander.la
# WANDER CONTROLLER
--- 5,9 ----
! lib_LTLIBRARIES = wander.la fasr.la source.la sink.la
# WANDER CONTROLLER
***************
*** 11,12 ****
--- 11,23 ----
wander_la_LDFLAGS = -module
+ # FASR controller
+ fasr_la_SOURCES = fasr.cc ../../libstage/stage.hh
+ fasr_la_LDFLAGS = -module
+
+ # resource source
+ source_la_SOURCES = source.cc ../../libstage/stage.hh
+ source_la_LDFLAGS = -module
+
+ # resource sink
+ sink_la_SOURCES = sink.cc ../../libstage/stage.hh
+ sink_la_LDFLAGS = -module
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
