Revision: 8565
http://playerstage.svn.sourceforge.net/playerstage/?rev=8565&view=rev
Author: rtv
Date: 2010-03-09 02:23:16 +0000 (Tue, 09 Mar 2010)
Log Message:
-----------
added option to ignore fiducial Z location, inspired by patch #2886830 by Tyler
Gunn. Removed print statement from ranger
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/libstage/model_fiducial.cc
code/stage/trunk/libstage/model_ranger.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-05 04:00:15 UTC (rev
8564)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-09 02:23:16 UTC (rev
8565)
@@ -255,6 +255,8 @@
// need at least these models to get any work done
// (pos must be good, as we used it in the initialization list)
assert( laser );
+ assert( fiducial );
+ assert( ranger );
assert( source );
assert( sink );
@@ -269,12 +271,13 @@
laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
laser->Subscribe();
+ // we subscribe to the fiducial device only while docking
fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate,
this );
- //fiducial->Subscribe();
- //pos->AddFlagIncrCallback( (stg_model_callback_t)FlagIncr, NULL );
- //pos->AddFlagDecrCallback( (stg_model_callback_t)FlagDecr, NULL );
+ // 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 )
@@ -421,6 +424,8 @@
if( Full() )
{
//printf( "fully charged, now back to work\n" );
+
+ ranger->Subscribe(); // enable the sonar to see behind us
mode = MODE_UNDOCK;
}
}
@@ -441,7 +446,9 @@
{
mode = MODE_WORK;
SetGoal( pos->GetFlagCount() ? sink : source );
+
fiducial->Unsubscribe();
+ ranger->Unsubscribe();
}
}
@@ -594,7 +601,15 @@
}
}
+
+ 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 )
{
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2010-03-05 04:00:15 UTC (rev
8564)
+++ code/stage/trunk/libstage/model_fiducial.cc 2010-03-09 02:23:16 UTC (rev
8565)
@@ -42,6 +42,7 @@
range_max 8.0
range_max_id 5.0
fov 3.14159
+ ignore_zloc
# model properties
size [ 0.1 0.1 0.1 ]
@@ -58,7 +59,9 @@
the maximum range at which the sensor can detect the ID of a fiducial, in
meters.
- fov <float>
the angular field of view of the scanner, in radians.
-
+- ignore_zloc <1/0>\n
+ default is 0. When set to 1, the fiducial finder ignores the z component
when checking a fiducial. Using the default behaviour, a short robot would not
been seen
+ by a tall robot's fiducial finder. With this flag set to 1, the fiducial
finder will see the shorter robot.
*/
ModelFiducial::ModelFiducial( World* world,
@@ -71,7 +74,8 @@
min_range( 0.0 ),
fov( M_PI ),
heading( 0 ),
- key( 0 )
+ key( 0 ),
+ ignore_zloc(false)
{
//PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n",
// id, typestr );
@@ -165,7 +169,7 @@
max_range_anon,
fiducial_raytrace_match,
NULL,
-
true ) );
+
! ignore_zloc ) );
Model* hitmod( ray.mod );
@@ -197,7 +201,7 @@
// if he's within ID range, get his fiducial.return value, else
// we see value 0
- fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0;
+ fid.id = range < max_range_id ? him->vis.fiducial_return : 0;
//PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
// him->Token(), him->vis.fiducial_return );
@@ -231,10 +235,11 @@
Model::Load();
// load fiducial-specific properties
- min_range = wf->ReadLength( wf_entity, "range_min", min_range );
- max_range_anon = wf->ReadLength( wf_entity, "range_max",
max_range_anon );
- max_range_id = wf->ReadLength( wf_entity, "range_max_id",
max_range_id );
- fov = wf->ReadAngle( wf_entity, "fov", fov );
+ min_range = wf->ReadLength( wf_entity, "range_min",
min_range );
+ max_range_anon = wf->ReadLength( wf_entity, "range_max",
max_range_anon );
+ max_range_id = wf->ReadLength( wf_entity, "range_max_id",
max_range_id );
+ fov = wf->ReadAngle ( wf_entity, "fov", fov
);
+ ignore_zloc = wf->ReadInt ( wf_entity, "ignore_zloc",
ignore_zloc);
}
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2010-03-05 04:00:15 UTC (rev
8564)
+++ code/stage/trunk/libstage/model_ranger.cc 2010-03-09 02:23:16 UTC (rev
8565)
@@ -272,7 +272,7 @@
s.range = std::max( ray.range, s.bounds_range.min );
- printf( " %.3f (%.3f) ", s.range, s.pose.a );
+ //printf( " %.3f (%.3f) ", s.range, s.pose.a );
}
puts("");
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2010-03-05 04:00:15 UTC (rev 8564)
+++ code/stage/trunk/libstage/stage.hh 2010-03-09 02:23:16 UTC (rev 8565)
@@ -2663,8 +2663,8 @@
stg_radians_t fov; ///< field of view
stg_radians_t heading; ///< center of field of view
int key; ///< /// only detect fiducials with a key that matches this
one (defaults 0)
+ bool ignore_zloc; ///< Are we ignoring the Z-loc of the fiducials we
detect compared to the fiducial detector?
-
/** Access the dectected fiducials. C++ style. */
std::vector<Fiducial>& GetFiducials() { return fiducials; }
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2010-03-05 04:00:15 UTC (rev 8564)
+++ code/stage/trunk/libstage/world.cc 2010-03-09 02:23:16 UTC (rev 8565)
@@ -533,6 +533,9 @@
sim_time += sim_interval;
+ FOR_EACH( it, active_velocity )
+ (*it)->UpdatePose();
+
// handle the zeroth queue synchronously in the main thread
ConsumeQueue( 0 );
@@ -566,9 +569,6 @@
// world callbacks
CallUpdateCallbacks();
- FOR_EACH( it, active_velocity )
- (*it)->UpdatePose();
-
FOR_EACH( it, active_energy )
(*it)->UpdateCharge();
Modified: code/stage/trunk/worlds/fasr2.world
===================================================================
--- code/stage/trunk/worlds/fasr2.world 2010-03-05 04:00:15 UTC (rev 8564)
+++ code/stage/trunk/worlds/fasr2.world 2010-03-09 02:23:16 UTC (rev 8565)
@@ -23,9 +23,9 @@
(
size [ 758.000 809.000 ]
- center [ 0.114 -16.418 ]
+ center [ -37.456 -47.457 ]
rotate [ 0 0 ]
- scale 5.533
+ scale 194.841
show_data 1
show_flags 1
@@ -145,7 +145,7 @@
size [ 0.050 0.150 0.100 ]
joules -1 # provides infinite energy
give_watts 2000
- fiducial_return 2 # look for this in the fiducial sensor
+ # fiducial_return 2 # look for this in the fiducial sensor
alwayson 1 # so we give charge without any explicit subscriber
@@ -153,6 +153,15 @@
obstacle_return 0
)
+ # the set-up target
+ model(
+ pose [0.500 0 0 0 ]
+ color "green"
+ size [0.100 0.100 0.015]
+ fiducial_return 2
+ obstacle_return 0
+ )
+
)
@@ -257,7 +266,7 @@
joules 100000
joules_capacity 400000
fiducial_return 0
- charging_bump( obstacle_return 0 fiducial( range_max_id 9 range_max 9 fov 180
pose [ 0 0 -0.050 0 ] ) )
+ charging_bump( obstacle_return 0 fiducial( ignore_zloc 1 range_max_id 9
range_max 9 fov 180 pose [ 0 0 -0.050 0 ] ) )
# small speed optimization
# trail_length 0
@@ -373,8 +382,8 @@
#cyanbot( pose [-2.198 18.082 0 -117.456] joules 400000 )
#cyanbot( pose [-6.301 18.782 0 -117.456] joules 400000 )
-greenbot( pose [-36.744 -47.036 0 117.485] joules 100000 )
-greenbot( pose [-37.050 -48.323 0 -85.614] joules 100000 )
+greenbot( pose [-36.953 -46.340 0 -41.980] joules 100000 )
+greenbot( pose [-37.595 -47.441 0 -119.991] joules 100000 )
greenbot( pose [-8.902 2.142 0 -65.139] joules 200000 )
greenbot( pose [-9.615 2.398 0 -73.426] joules 200000 )
greenbot( pose [-8.619 0.004 0 -93.488] joules 200000 )
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
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
