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 Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit