Revision: 7331
http://playerstage.svn.sourceforge.net/playerstage/?rev=7331&view=rev
Author: rtv
Date: 2009-02-13 09:43:20 +0000 (Fri, 13 Feb 2009)
Log Message:
-----------
basic gripper working - needs thorough testing
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/model_callbacks.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 07:12:04 UTC (rev
7330)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 09:43:20 UTC (rev
7331)
@@ -59,21 +59,8 @@
double charger_range;
double charger_heading;
nav_mode_t mode;
+ bool at_dest;
- static int LaserUpdate( ModelLaser* mod, Robot* robot );
- static int PositionUpdate( ModelPosition* mod, Robot* robot );
- static int FiducialUpdate( ModelFiducial* mod, Robot* robot );
- static int BlobFinderUpdate( ModelBlobfinder* mod, Robot* robot );
-
- void Dock();
- void Work();
- void UnDock();
- bool ObstacleAvoid();
-
- // predicate that indicates if we need to charge
- bool Hungry();
- bool Full();
-
public:
Robot( ModelPosition* pos,
Model* source,
@@ -94,7 +81,8 @@
charger_bearing(0),
charger_range(0),
charger_heading(0),
- mode(MODE_WORK)
+ mode(MODE_WORK),
+ at_dest( false )
{
// need at least these models to get any work done
// (pos must be good, as we used it in the initialization list)
@@ -104,381 +92,421 @@
// PositionUpdate() checks to see if we reached source or sink
pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+ pos->Subscribe();
// LaserUpdate() controls the robot, by reading from laser and
// writing to position
laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ laser->Subscribe();
- // trivial demos
-
- if( fiducial ) // optional
- fiducial->AddUpdateCallback(
(stg_model_callback_t)FiducialUpdate, this );
-
+ fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate,
this );
+ fiducial->Subscribe();
+
+ //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate,
this );
+ gripper->Subscribe();
+
if( blobfinder ) // optional
- blobfinder->AddUpdateCallback(
(stg_model_callback_t)BlobFinderUpdate, this );
- }
-};
-
-
-
-// Stage calls this when the model starts up
-extern "C" int Init( Model* mod )
-{
- Robot* robot = new Robot( (ModelPosition*)mod,
-
mod->GetWorld()->GetModel( "source" ),
-
mod->GetWorld()->GetModel( "sink" ) );
-
- return 0; //ok
+ {
+ blobfinder->AddUpdateCallback(
(stg_model_callback_t)BlobFinderUpdate, this );
+ blobfinder->Subscribe();
+ }
}
-
-
-void Robot::Dock()
-{
- // close the grippers so they can be pushed into the charger
- ModelGripper::data_t gripper_data = gripper->GetData();
+ void Dock()
+ {
+ // close the grippers so they can be pushed into the charger
+ ModelGripper::data_t gripper_data = gripper->GetData();
- if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED )
- gripper->CommandClose();
- else if( gripper_data.lift != ModelGripper::LIFT_UP )
- gripper->CommandUp();
+ if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED )
+ gripper->CommandClose();
+ else if( gripper_data.lift != ModelGripper::LIFT_UP )
+ gripper->CommandUp();
- if( charger_ahoy )
- {
- double a_goal = normalize( charger_bearing );
+ if( charger_ahoy )
+ {
+ double a_goal = normalize( charger_bearing );
-// if( pos->Stalled() )
-// {
-// puts( "stalled. stopping" );
-// pos->Stop();
-// }
-// else
+ // if( pos->Stalled() )
+ // {
+ // puts( "stalled. stopping" );
+ // pos->Stop();
+ // }
+ // else
- if( charger_range > 0.5 )
- {
- if( !ObstacleAvoid() )
- {
- pos->SetXSpeed( cruisespeed );
- pos->SetTurnSpeed( a_goal );
- }
- }
- else
- {
- pos->SetTurnSpeed( a_goal );
- pos->SetXSpeed( 0.02 ); // creep towards it
+ if( charger_range > 0.5 )
+ {
+ if( !ObstacleAvoid() )
+ {
+ pos->SetXSpeed( cruisespeed );
+ pos->SetTurnSpeed( a_goal );
+ }
+ }
+ else
+ {
+ pos->SetTurnSpeed( a_goal );
+ pos->SetXSpeed( 0.02 ); // creep towards it
- if( charger_range < 0.08 ) // close enough
- pos->Stop();
+ if( charger_range < 0.08 ) // close enough
+ pos->Stop();
- if( pos->Stalled() ) // touching
- pos->SetXSpeed( -0.01 ); // back off a bit
+ if( pos->Stalled() ) // touching
+ pos->SetXSpeed( -0.01 ); // back off a bit
- }
- }
- else
- {
- //printf( "docking but can't see a charger\n" );
- pos->Stop();
- mode = MODE_WORK; // should get us back on track eventually
- }
+ }
+ }
+ else
+ {
+ //printf( "docking but can't see a charger\n" );
+ pos->Stop();
+ mode = MODE_WORK; // should get us back on track eventually
+ }
- // if the battery is charged, go back to work
- if( Full() )
- {
- //printf( "fully charged, now back to work\n" );
- mode = MODE_UNDOCK;
- }
-}
+ // if the battery is charged, go back to work
+ if( Full() )
+ {
+ //printf( "fully charged, now back to work\n" );
+ mode = MODE_UNDOCK;
+ }
+ }
-void Robot::UnDock()
-{
- const stg_meters_t gripper_distance = 0.2;
- const stg_meters_t back_off_distance = 0.3;
- const stg_meters_t back_off_speed = -0.05;
+ void UnDock()
+ {
+ const stg_meters_t gripper_distance = 0.2;
+ const stg_meters_t back_off_distance = 0.3;
+ const stg_meters_t back_off_speed = -0.05;
- // back up a bit
- if( charger_range < back_off_distance )
- pos->SetXSpeed( back_off_speed );
- else
- pos->SetXSpeed( 0.0 );
+ // back up a bit
+ if( charger_range < back_off_distance )
+ pos->SetXSpeed( back_off_speed );
+ else
+ pos->SetXSpeed( 0.0 );
- // once we have backed off a bit, open and lower the gripper
- ModelGripper::data_t gripper_data = gripper->GetData();
- if( charger_range > gripper_distance )
- {
- if( gripper_data.paddles != ModelGripper::PADDLE_OPEN )
- gripper->CommandOpen();
- else if( gripper_data.lift != ModelGripper::LIFT_DOWN )
- gripper->CommandDown();
- }
+ // once we have backed off a bit, open and lower the gripper
+ ModelGripper::data_t gripper_data = gripper->GetData();
+ if( charger_range > gripper_distance )
+ {
+ if( gripper_data.paddles != ModelGripper::PADDLE_OPEN )
+ gripper->CommandOpen();
+ else if( gripper_data.lift != ModelGripper::LIFT_DOWN )
+ gripper->CommandDown();
+ }
- // if the gripper is down and open and we're away from the charger, undock
is finished
- if( gripper_data.paddles == ModelGripper::PADDLE_OPEN &&
- gripper_data.lift == ModelGripper::LIFT_DOWN &&
- charger_range > back_off_distance )
- mode = MODE_WORK;
-}
+ // if the gripper is down and open and we're away from the charger,
undock is finished
+ if( gripper_data.paddles == ModelGripper::PADDLE_OPEN &&
+ gripper_data.lift == ModelGripper::LIFT_DOWN &&
+ charger_range > back_off_distance )
+ mode = MODE_WORK;
+ }
-bool Robot::ObstacleAvoid()
-{
- bool obstruction = false;
- bool stop = false;
+ bool ObstacleAvoid()
+ {
+ 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;
+ // find the closest distance to the left and right and check if
+ // there's anything in front
+ double minleft = 1e6;
+ double minright = 1e6;
- // Get the data
- uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+ // Get the data
+ uint32_t sample_count=0;
+ stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
- for (uint32_t i = 0; i < sample_count; i++)
- {
- if( verbose ) printf( "%.3f ", scan[i].range );
+ 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)
- {
- if( verbose ) puts( " obstruction!" );
- obstruction = true;
- }
+ if( (i > (sample_count/4))
+ && (i < (sample_count - (sample_count/4)))
+ && scan[i].range < minfrontdistance)
+ {
+ if( verbose ) puts( " obstruction!" );
+ obstruction = true;
+ }
- if( scan[i].range < stopdist )
- {
- if( verbose ) puts( " stopping!" );
- stop = true;
- }
+ if( scan[i].range < stopdist )
+ {
+ if( verbose ) puts( " stopping!" );
+ stop = true;
+ }
- if( i > sample_count/2 )
- minleft = MIN( minleft, scan[i].range );
- else
- minright = MIN( minright, scan[i].range );
- }
+ if( i > sample_count/2 )
+ minleft = MIN( minleft, scan[i].range );
+ else
+ minright = MIN( minright, scan[i].range );
+ }
- if( verbose )
- {
- puts( "" );
- printf( "minleft %.3f \n", minleft );
- printf( "minright %.3f\n ", minright );
- }
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
- if( obstruction || stop || (avoidcount>0) )
- {
- if( verbose ) printf( "Avoid %d\n", avoidcount );
+ if( obstruction || stop || (avoidcount>0) )
+ {
+ if( verbose ) printf( "Avoid %d\n", avoidcount );
- pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+ pos->SetXSpeed( stop ? 0.0 : avoidspeed );
- /* once we start avoiding, select a turn direction and stick
- with it for a few iterations */
- if( avoidcount < 1 )
- {
- if( verbose ) puts( "Avoid START" );
- avoidcount = random() % avoidduration + avoidduration;
+ /* once we start avoiding, select a turn direction and stick
+ with it for a few iterations */
+ if( avoidcount < 1 )
+ {
+ if( verbose ) puts( "Avoid START" );
+ avoidcount = random() % avoidduration +
avoidduration;
- if( minleft < minright )
- {
- pos->SetTurnSpeed( -avoidturn );
- if( verbose ) printf( "turning right %.2f\n",
-avoidturn );
- }
- else
- {
- pos->SetTurnSpeed( +avoidturn );
- if( verbose ) printf( "turning left %2f\n",
+avoidturn );
- }
- }
+ if( minleft < minright )
+ {
+ pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right
%.2f\n", -avoidturn );
+ }
+ else
+ {
+ pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left
%2f\n", +avoidturn );
+ }
+ }
- avoidcount--;
+ avoidcount--;
- return true; // busy avoding obstacles
- }
+ return true; // busy avoding obstacles
+ }
- return false; // didn't have to avoid anything
-}
+ return false; // didn't have to avoid anything
+ }
-void Robot::Work()
-{
- if( ! ObstacleAvoid() )
- {
- if( verbose ) puts( "Cruise" );
+ void Work()
+ {
+ if( ! ObstacleAvoid() )
+ {
+ if( verbose ) puts( "Cruise" );
- //avoidcount = 0;
- pos->SetXSpeed( cruisespeed );
+ ModelGripper::data_t gdata = gripper->GetData();
+
+ //avoidcount = 0;
+ pos->SetXSpeed( cruisespeed );
- Pose pose = pos->GetPose();
+ Pose pose = pos->GetPose();
- int x = (pose.x + 8) / 4;
- int y = (pose.y + 8) / 4;
+ int x = (pose.x + 8) / 4;
+ int y = (pose.y + 8) / 4;
- // oh what an awful bug - 5 hours to track this down. When using
- // this controller in a world larger than 8*8 meters, a_goal can
- // sometimes be NAN. Causing trouble WAY upstream.
- if( x > 3 ) x = 3;
- if( y > 3 ) y = 3;
- if( x < 0 ) x = 0;
- if( y < 0 ) y = 0;
+ // oh what an awful bug - 5 hours to track this down. When
using
+ // this controller in a world larger than 8*8 meters, a_goal
can
+ // sometimes be NAN. Causing trouble WAY upstream.
+ if( x > 3 ) x = 3;
+ if( y > 3 ) y = 3;
+ if( x < 0 ) x = 0;
+ if( y < 0 ) y = 0;
- double a_goal =
- dtor( pos->GetFlagCount() ? have[y][x] : need[y][x] );
+ double a_goal =
+ dtor( ( pos->GetFlagCount() || gdata.stack_count ) ?
have[y][x] : need[y][x] );
- // if we are low on juice - find the direction to the recharger
instead
- if( Hungry() )
- {
- //puts( "hungry - using refuel map" );
+ // if we are low on juice - find the direction to the
recharger instead
+ if( Hungry() )
+ {
+ //puts( "hungry - using refuel map" );
- // use the refuel map
- a_goal = dtor( refuel[y][x] );
+ // use the refuel map
+ a_goal = dtor( refuel[y][x] );
- if( charger_ahoy ) // I see a charger while hungry!
- mode = MODE_DOCK;
- }
+ if( charger_ahoy ) // I see a charger while
hungry!
+ mode = MODE_DOCK;
+ }
+ else
+ {
+ if( ! at_dest )
+ {
+ if( gdata.beam[0] ) // inner break
beam broken
+ gripper->CommandClose();
+ }
+ }
+
+ assert( ! isnan(a_goal ) );
+ assert( ! isnan(pose.a ) );
+
+ double a_error = normalize( a_goal - pose.a );
- assert( ! isnan(a_goal ) );
- assert( ! isnan(pose.a ) );
+ assert( ! isnan(a_error) );
- double a_error = normalize( a_goal - pose.a );
-
- assert( ! isnan(a_error) );
-
- pos->SetTurnSpeed( a_error );
- }
-}
+ pos->SetTurnSpeed( a_error );
+ }
+ }
-// inspect the laser data and decide what to do
-int Robot::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 );
+ // 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;
+ if( laser->GetSamples(NULL) == NULL )
+ return 0;
- switch( robot->mode )
- {
- case MODE_DOCK:
- //puts( "DOCK" );
- robot->Dock();
- break;
+ switch( robot->mode )
+ {
+ case MODE_DOCK:
+ //puts( "DOCK" );
+ robot->Dock();
+ break;
- case MODE_WORK:
- //puts( "WORK" );
- robot->Work();
- break;
+ case MODE_WORK:
+ //puts( "WORK" );
+ robot->Work();
+ break;
- case MODE_UNDOCK:
- //puts( "UNDOCK" );
- robot->UnDock();
- break;
+ case MODE_UNDOCK:
+ //puts( "UNDOCK" );
+ robot->UnDock();
+ break;
- default:
- printf( "unrecognized mode %u\n", robot->mode );
- }
+ default:
+ printf( "unrecognized mode %u\n", robot->mode );
+ }
- //if( robot->charger_ahoy )
- //return 1;
- //else
+ //if( robot->charger_ahoy )
+ //return 1;
+ //else
return 0;
-}
+ }
-bool Robot::Hungry()
-{
- return( pos->FindPowerPack()->ProportionRemaining() < 0.25 );
-}
+ bool Hungry()
+ {
+ return( pos->FindPowerPack()->ProportionRemaining() < 0.25 );
+ }
-bool Robot::Full()
-{
- return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
-}
+ bool Full()
+ {
+ return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
+ }
-int Robot::PositionUpdate( ModelPosition* pos, Robot* robot )
-{
- Pose pose = pos->GetPose();
+ static int PositionUpdate( ModelPosition* pos, Robot* robot )
+ {
+ Pose pose = pos->GetPose();
- //printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
- // pose.x, pose.y, pose.z, pose.a );
+ //printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
+ // pose.x, pose.y, pose.z, pose.a );
- //pose.z += 0.0001;
- //robot->pos->SetPose( pose );
+ //pose.z += 0.0001;
+ //robot->pos->SetPose( pose );
- if( pos->GetFlagCount() < payload &&
- hypot( -7-pose.x, -7-pose.y ) < 2.0 )
- {
- if( ++robot->work_get > workduration )
- {
- // protect source from concurrent access
- robot->source->Lock();
+ if( pos->GetFlagCount() < payload &&
+ hypot( -7-pose.x, -7-pose.y ) < 2.0 )
+ {
+ if( ++robot->work_get > workduration )
+ {
+ // protect source from concurrent access
+ robot->source->Lock();
- // transfer a chunk from source to robot
- pos->PushFlag( robot->source->PopFlag() );
- robot->source->Unlock();
+ // transfer a chunk from source to robot
+ pos->PushFlag( robot->source->PopFlag() );
+ robot->source->Unlock();
- robot->work_get = 0;
- }
- }
+ robot->work_get = 0;
+ }
+ }
- if( hypot( 7-pose.x, 7-pose.y ) < 1.0 )
- {
- if( ++robot->work_put > workduration )
- {
- // protect sink from concurrent access
- robot->sink->Lock();
+ robot->at_dest = false;
- //puts( "dropping" );
- // transfer a chunk between robot and goal
- robot->sink->PushFlag( pos->PopFlag() );
- robot->sink->Unlock();
+ if( hypot( 7-pose.x, 7-pose.y ) < 1.0 )
+ {
+ robot->at_dest = true;
- robot->work_put = 0;
- }
- }
+ if( ++robot->work_put > workduration )
+ {
+ // protect sink from concurrent access
+ robot->sink->Lock();
+
+ //puts( "dropping" );
+ // transfer a chunk between robot and goal
+ robot->sink->PushFlag( pos->PopFlag() );
+ robot->sink->Unlock();
+
+ robot->work_put = 0;
+
+ robot->gripper->CommandOpen();
+ }
+ }
- return 0; // run again
-}
+ return 0; // run again
+ }
-int Robot::FiducialUpdate( ModelFiducial* mod, Robot* robot )
-{
- robot->charger_ahoy = false;
+ static int FiducialUpdate( ModelFiducial* mod, Robot* robot )
+ {
+ robot->charger_ahoy = false;
- for( unsigned int i = 0; i < mod->fiducial_count; i++ )
- {
- stg_fiducial_t* f = &mod->fiducials[i];
+ for( unsigned int i = 0; i < mod->fiducial_count; i++ )
+ {
+ stg_fiducial_t* f = &mod->fiducials[i];
- //printf( "fiducial %d is %d at %.2f m %.2f radians\n",
- // i, f->id, f->range, f->bearing );
+ //printf( "fiducial %d is %d at %.2f m %.2f radians\n",
+ // i, f->id, f->range, f->bearing );
- if( f->id == 2 ) // I see a charging station
- {
- // record that I've seen it and where it is
- robot->charger_ahoy = true;
- robot->charger_bearing = f->bearing;
- robot->charger_range = f->range;
- robot->charger_heading = f->geom.a;
+ if( f->id == 2 ) // I see a charging station
+ {
+ // record that I've seen it and where it is
+ robot->charger_ahoy = true;
+ robot->charger_bearing = f->bearing;
+ robot->charger_range = f->range;
+ robot->charger_heading = f->geom.a;
- //printf( "charger at %.2f radians\n",
robot->charger_bearing );
- break;
- }
- }
+ //printf( "charger at %.2f radians\n",
robot->charger_bearing );
+ break;
+ }
+ }
- return 0; // run again
-}
+ return 0; // run again
+ }
-int Robot::BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot )
+ static int BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot )
+ {
+ unsigned int blob_count = 0;
+ stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
+
+ if( blobs && (blob_count>0) )
+ {
+ printf( "%s sees %u blobs\n", blobmod->Token(), blob_count );
+ }
+
+ return 0;
+ }
+
+ static int GripperUpdate( ModelGripper* grip, Robot* robot )
+ {
+ ModelGripper::data_t gdata = grip->GetData();
+
+ printf( "BREAKBEAMS %s %s\n",
+ gdata.beam[0] ? gdata.beam[0]->Token() :
"<null>",
+ gdata.beam[1] ? gdata.beam[1]->Token() :
"<null>" );
+
+ printf( "CONTACTS %s %s\n",
+ gdata.contact[0] ? gdata.contact[0]->Token() :
"<null>",
+ gdata.contact[1] ? gdata.contact[1]->Token() :
"<null>");
+
+
+ return 0;
+ }
+
+
+};
+
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod )
{
- unsigned int blob_count = 0;
- stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
+ Robot* robot = new Robot( (ModelPosition*)mod,
+
mod->GetWorld()->GetModel( "source" ),
+
mod->GetWorld()->GetModel( "sink" ) );
+
+ return 0; //ok
+}
- if( blobs && (blob_count>0) )
- {
- printf( "%s sees %u blobs\n", blobmod->Token(), blob_count );
- }
- return 0;
-}
+
Modified: code/stage/trunk/libstage/model_callbacks.cc
===================================================================
--- code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 07:12:04 UTC
(rev 7330)
+++ code/stage/trunk/libstage/model_callbacks.cc 2009-02-13 09:43:20 UTC
(rev 7331)
@@ -23,12 +23,6 @@
// and replace the list in the hash table
g_hash_table_insert( callbacks, key, cb_list );
-
- // if the callback was an update function, add this model to the
- // world's list of models that need updating (if it's not there
- // already)
- //if( address == (void*)&update )
- //stg_world_start_updating_model( world, this );
}
@@ -76,9 +70,7 @@
// return the number of callbacks now in the list. Useful for
// detecting when the list is empty.
- //return g_list_length( cb_list );
-
- return 0;
+ return g_list_length( cb_list );
}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-02-13 07:12:04 UTC (rev 7330)
+++ code/stage/trunk/libstage/stage.hh 2009-02-13 09:43:20 UTC (rev 7331)
@@ -2091,13 +2091,13 @@
void AddUpdateCallback( stg_model_callback_t cb, void* user )
{
AddCallback( &hooks.update, cb, user );
- Subscribe(); // if attaching a callback here, assume we want
updates to happen
+ //Subscribe(); // if attaching a callback here, assume we want
updates to happen
}
void RemoveUpdateCallback( stg_model_callback_t cb )
{
RemoveCallback( &hooks.update, cb );
- Unsubscribe();
+ //Unsubscribe();
}
/** named-property interface
@@ -2326,6 +2326,7 @@
CMD_DOWN
};
+
/** gripper configuration
*/
struct config_t
@@ -2338,16 +2339,19 @@
double paddle_position; ///< 0.0 = full open, 1.0 full closed
double lift_position; ///< 0.0 = full down, 1.0 full up
- stg_meters_t inner_break_beam_inset; ///< distance from the end
of the paddle
- stg_meters_t outer_break_beam_inset; ///< distance from the end
of the paddle
bool paddles_stalled; // true iff some solid object stopped
// the paddles closing or opening
- GSList *grip_stack; ///< stack of items gripped
+ GList*grip_stack; ///< stack of items gripped
int grip_stack_size; ///< maximum number of objects in stack,
or -1 for unlimited
double close_limit; ///< How far the gripper can close. If <
1.0, the gripper has its mouth full.
bool autosnatch; ///< if true, cycle the gripper through
open-close-up-down automatically
+
+ double break_beam_inset[2]; ///< distance from the end of the
paddle
+
+ Model* beam[2]; ///< points to a model detected by the beams
+ Model* contact[2]; ///< pointers to a model detected by the contacts
};
@@ -2361,17 +2365,14 @@
double paddle_position; ///< 0.0 = full open, 1.0 full closed
double lift_position; ///< 0.0 = full down, 1.0 full up
- stg_bool_t inner_break_beam; ///< non-zero iff beam is broken
- stg_bool_t outer_break_beam; ///< non-zero iff beam is broken
- stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something
-
stg_bool_t paddles_stalled; // true iff some solid object stopped
// the paddles closing or opening
+ Model* beam[2]; ///< points to a model detected by the beams
+ Model* contact[2]; ///< pointers to a model detected by the contacts
+
int stack_count; ///< number of objects in stack
-
-
};
private:
@@ -2380,6 +2381,8 @@
void FixBlocks();
void PositionPaddles();
+ void UpdateBreakBeams();
+ void UpdateContacts();
config_t cfg;
cmd_t cmd;
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-02-13 07:12:04 UTC (rev 7330)
+++ code/stage/trunk/worlds/fasr.world 2009-02-13 09:43:20 UTC (rev 7331)
@@ -22,9 +22,9 @@
(
size [ 902.000 856.000 ]
- center [ 5.355 5.319 ]
- rotate [ 30.000 26.000 ]
- scale 126.761
+ center [ 3.982 5.051 ]
+ rotate [ 0 0 ]
+ scale 168.655
pcam_loc [ 0 -4.000 2.000 ]
pcam_angle [ 70.000 0 ]
@@ -79,12 +79,12 @@
fiducial_return 0
# charging_bump( fiducial( range 3 pose [ 0 0 -0.100 0 ] ) )
- gripper( pose [0.25 0 -0.22 0]
+ gripper( pose [0.250 0 -0.220 0]
take_watts 1000.0
fiducial( range 3 )
# paddles [ "closed" "up" ]
obstacle_return 0 # cheating for simplicity
- autosnatch 0
+ # autosnatch 1
)
)
@@ -94,14 +94,14 @@
color "purple"
# side blocks to restrict view angle
- model( color "purple" size [0.100 0.050 0.400] pose [ 0 0.100 0 0 ] )
- model( color "purple" size [0.100 0.050 0.400] pose [ 0 -0.100 0 0 ] )
+ model( color "purple" size [0.100 0.050 0.250] pose [ 0 0.100 0 0 ] )
+ model( color "purple" size [0.100 0.050 0.250] pose [ 0 -0.100 0 0 ] )
# the charging block
model(
pose [ 0.010 0 0 0 ]
color "yellow"
- size [ 0.050 0.200 0.150 ]
+ size [ 0.050 0.200 0.100 ]
joules -1 # provides infinite energy
give_watts 1000
fiducial_return 2 # look for this in the fiducial sensor
@@ -114,7 +114,46 @@
charge_station( pose [ 7.931 -3.367 0 0 ] )
charge_station( pose [ 7.931 -4.444 0 0 ] )
-autorob( pose [4.144 6.834 0 -98.076] joules 300000 )
+define puck model (
+ size [0.120 0.120 0.100]
+ color "violet"
+ gripper_return 1
+ obstacle_return 0
+)
+
+puck( pose [ 4.519 6.829 0 0 ] )
+puck( pose [ 4.088 7.018 0 0 ] )
+puck( pose [ 4.409 7.207 0 0 ] )
+puck( pose [ 3.589 7.324 0 0 ] )
+puck( pose [ 3.729 6.893 0 0 ] )
+puck( pose [ 3.714 7.683 0 0 ] )
+puck( pose [ 3.869 7.275 0 0 ] )
+puck( pose [ 4.039 6.719 0 0 ] )
+puck( pose [ 4.269 6.704 0 0 ] )
+puck( pose [ 3.752 7.101 0 0 ] )
+puck( pose [ 4.243 7.033 0 0 ] )
+puck( pose [ 4.666 7.052 0 0 ] )
+puck( pose [ 3.820 6.629 0 0 ] )
+
+puck( pose [ 3.930 6.381 0 0 ] )
+puck( pose [ 4.560 6.269 0 0 ] )
+puck( pose [ 4.391 5.814 0 0 ] )
+puck( pose [ 3.475 5.926 0 0 ] )
+puck( pose [ 3.205 6.595 0 0 ] )
+puck( pose [ 3.549 6.393 0 0 ] )
+puck( pose [ 3.957 5.640 0 0 ] )
+puck( pose [ 3.825 5.781 0 0 ] )
+puck( pose [ 4.789 5.122 0 0 ] )
+puck( pose [ 4.167 5.850 0 0 ] )
+puck( pose [ 3.652 6.101 0 0 ] )
+puck( pose [ 3.957 5.966 0 0 ] )
+puck( pose [ 4.685 5.982 0 0 ] )
+puck( pose [ 4.274 6.152 0 0 ] )
+puck( pose [ 4.380 6.491 0 0 ] )
+puck( pose [ 4.870 6.449 0 0 ] )
+puck( pose [ 3.588 6.635 0 0 ] )
+
+autorob( pose [5.462 7.216 0 -163.478] joules 300000 )
autorob( pose [7.574 6.269 0 -111.715] joules 100000 )
autorob( pose [5.615 6.185 0 107.666] joules 200000 )
autorob( pose [7.028 6.502 0 -128.279] joules 400000 )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
