Revision: 7152
http://playerstage.svn.sourceforge.net/playerstage/?rev=7152&view=rev
Author: rtv
Date: 2008-11-15 03:11:44 +0000 (Sat, 15 Nov 2008)
Log Message:
-----------
major changes for 3.1
Modified Paths:
--------------
code/stage/trunk/CMakeLists.txt
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/examples/ctrl/sink.cc
code/stage/trunk/examples/ctrl/source.cc
code/stage/trunk/examples/ctrl/wander.cc
Modified: code/stage/trunk/CMakeLists.txt
===================================================================
--- code/stage/trunk/CMakeLists.txt 2008-11-15 03:11:20 UTC (rev 7151)
+++ code/stage/trunk/CMakeLists.txt 2008-11-15 03:11:44 UTC (rev 7152)
@@ -1,8 +1,8 @@
PROJECT(Stage)
SET( V_MAJOR 3 )
-SET( V_MINOR 0 )
-SET( V_BUGFIX 1 )
+SET( V_MINOR 1 )
+SET( V_BUGFIX 0 )
SET( VERSION ${V_MAJOR}.${V_MINOR}.${V_BUGFIX} )
SET( APIVERSION ${V_MAJOR}.${V_MINOR} )
@@ -57,7 +57,7 @@
MESSAGE( STATUS "Checking for required libraries..." )
SET( INDENT " * " )
-pkg_search_module( GLIB REQUIRED glib-2.0 )
+pkg_search_module( GLIB REQUIRED gthread-2.0 )
IF( GLIB_FOUND )
MESSAGE( ${INDENT} "Glib version ${GLIB_VERSION} detected at ${GLIB_PREFIX}"
)
MESSAGE( " GLIB_CFLAGS = ${GLIB_CFLAGS}" )
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2008-11-15 03:11:20 UTC (rev
7151)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-15 03:11:44 UTC (rev
7152)
@@ -45,9 +45,10 @@
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->work_get = 0;
robot->work_put = 0;
@@ -69,7 +70,7 @@
robot->sink = mod->GetWorld()->GetModel( "sink" );
assert(robot->sink);
-
+
return 0; //ok
}
@@ -93,38 +94,61 @@
for (uint32_t i = 0; i < sample_count; i++)
{
+
+ if( verbose ) printf( "%.3f ", scan[i].range );
+
if( (i > (sample_count/4))
- && (i < (sample_count - (sample_count/4)))
- && scan[i].range < minfrontdistance)
- obstruction = true;
-
+ && (i < (sample_count - (sample_count/4)))
+ && scan[i].range < minfrontdistance)
+ {
+ if( verbose ) puts( " obstruction!" );
+ obstruction = true;
+ }
+
if( scan[i].range < stopdist )
- stop = true;
+ {
+ if( verbose ) puts( " stopping!" );
+ stop = true;
+ }
if( i > sample_count/2 )
- minleft = MIN( minleft, scan[i].range );
+ minleft = MIN( minleft, scan[i].range );
else
- minright = MIN( minright, scan[i].range );
+ minright = MIN( minright, scan[i].range );
}
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
+
if( obstruction || stop || (robot->avoidcount>0) )
{
- if( verbose ) puts( "Avoid" );
+ 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( verbose ) puts( "Avoid START" );
+ if( verbose ) puts( "Avoid START" );
robot->avoidcount = random() % avoidduration + avoidduration;
-
- if( minleft < minright )
- robot->pos->SetTurnSpeed( -avoidturn );
- else
- robot->pos->SetTurnSpeed( +avoidturn );
+
+ if( minleft < minright )
+ {
+ robot->pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right %.2f\n",
-avoidturn );
+ }
+ else
+ {
+ robot->pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left %2f\n",
+avoidturn );
+ }
}
-
+
robot->avoidcount--;
}
else
@@ -140,18 +164,19 @@
int y = (pose.y + 8) / 4;
double a_goal =
- dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] );
+ 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();
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
@@ -159,28 +184,40 @@
//pose.z += 0.0001;
//robot->pos->SetPose( pose );
-
+
if( robot->pos->GetFlagCount() < payload &&
hypot( -7-pose.x, -7-pose.y ) < 2.0 )
{
if( ++robot->work_get > workduration )
- {
- robot->pos->PushFlag( robot->source->PopFlag() );
- robot->work_get = 0;
- }
+ {
+ // protect source from concurrent access
+ robot->source->Lock();
+
+ // transfer a chunk from source to robot
+ robot->pos->PushFlag( robot->source->PopFlag() );
+ robot->source->Unlock();
+
+ robot->work_get = 0;
+ }
}
if( hypot( 7-pose.x, 7-pose.y ) < 1.0 )
{
if( ++robot->work_put > workduration )
- {
- //puts( "dropping" );
- // transfer a chunk between robot and goal
- robot->sink->PushFlag( robot->pos->PopFlag() );
- robot->work_put = 0;
- }
+ {
+ // protect sink from concurrent access
+ robot->sink->Lock();
+
+ //puts( "dropping" );
+ // transfer a chunk between robot and goal
+ robot->sink->PushFlag( robot->pos->PopFlag() );
+ robot->sink->Unlock();
+
+ robot->work_put = 0;
+ }
}
+
return 0; // run again
}
Modified: code/stage/trunk/examples/ctrl/sink.cc
===================================================================
--- code/stage/trunk/examples/ctrl/sink.cc 2008-11-15 03:11:20 UTC (rev
7151)
+++ code/stage/trunk/examples/ctrl/sink.cc 2008-11-15 03:11:44 UTC (rev
7152)
@@ -20,9 +20,13 @@
// inspect the laser data and decide what to do
int Update( StgModel* mod, void* dummy )
{
+ // protect access to this model from other controllers
+ mod->Lock();
if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
mod->PopFlag();
+
+ mod->Unlock();
return 0; // run again
}
Modified: code/stage/trunk/examples/ctrl/source.cc
===================================================================
--- code/stage/trunk/examples/ctrl/source.cc 2008-11-15 03:11:20 UTC (rev
7151)
+++ code/stage/trunk/examples/ctrl/source.cc 2008-11-15 03:11:44 UTC (rev
7152)
@@ -10,21 +10,24 @@
// 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 ), flagsz ) );
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 )
{
-
+ // protect access to this model from other controllers
+ mod->Lock();
+
if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
mod->PushFlag( new StgFlag( stg_color_pack( 1,1,0,0), flagsz ) );
+
+ mod->Unlock();
return 0; // run again
}
Modified: code/stage/trunk/examples/ctrl/wander.cc
===================================================================
--- code/stage/trunk/examples/ctrl/wander.cc 2008-11-15 03:11:20 UTC (rev
7151)
+++ code/stage/trunk/examples/ctrl/wander.cc 2008-11-15 03:11:44 UTC (rev
7152)
@@ -3,9 +3,11 @@
const double cruisespeed = 0.4;
const double avoidspeed = 0.05;
-const double avoidturn = 1.0;
-const double minfrontdistance = 0.8;
+const double avoidturn = 0.5;
+const double minfrontdistance = 0.6;
const bool verbose = false;
+const double stopdist = 0.3;
+const int avoidduration = 10;
typedef struct
{
@@ -33,6 +35,7 @@
return 0; //ok
}
+
// inspect the laser data and decide what to do
int LaserUpdate( StgModel* mod, robot_t* robot )
{
@@ -41,44 +44,71 @@
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( scan[i].range < minfrontdistance)
- obstruction = true;
+
+ if( verbose ) printf( "%.3f ", scan[i].range );
+
+ if( (i > (sample_count/3))
+ && (i < (sample_count - (sample_count/3)))
+ && scan[i].range < minfrontdistance)
+ {
+ if( verbose ) puts( " obstruction!" );
+ obstruction = true;
+ }
+
+ if( scan[i].range < stopdist )
+ {
+ if( verbose ) puts( " stopping!" );
+ stop = true;
+ }
if( i > sample_count/2 )
- minleft = MIN( minleft, scan[i].range );
+ minleft = MIN( minleft, scan[i].range );
else
- minright = MIN( minright, scan[i].range );
+ minright = MIN( minright, scan[i].range );
}
- if( obstruction || robot->avoidcount )
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
+
+ if( obstruction || stop || (robot->avoidcount>0) )
{
- if( verbose ) puts( "Avoid" );
-
- robot->pos->SetXSpeed( avoidspeed );
+ 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 == 0 )
+ if( robot->avoidcount < 1 )
{
- if( verbose ) puts( "Avoid START" );
- robot->avoidcount = 5;
-
- if( minleft < minright )
- robot->pos->SetTurnSpeed( -avoidturn );
- else
- robot->pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) puts( "Avoid START" );
+ robot->avoidcount = random() % avoidduration + avoidduration;
+
+ if( minleft < minright )
+ {
+ robot->pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right %.2f\n",
-avoidturn );
+ }
+ else
+ {
+ robot->pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left %2f\n",
+avoidturn );
+ }
}
-
+
robot->avoidcount--;
}
else
@@ -87,23 +117,9 @@
robot->avoidcount = 0;
robot->pos->SetXSpeed( cruisespeed );
-
- /* update turnrate every few updates */
- if( robot->randcount == 0 )
- {
- if( verbose )puts( "Random turn" );
-
- /* make random int tween -30 and 30 */
- //newturnrate = dtor( rand() % 61 - 30 );
-
- robot->randcount = 20;
-
- robot->pos->SetTurnSpeed( dtor( rand() % 11 - 5 ) );
- }
-
- robot->randcount--;
+ robot->pos->SetTurnSpeed( 0 );
}
-
+
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
