Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 389 lines (297 sloc) 10.771 kb
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
1 ///////////////////////////////////////////////////////////////////////////
2 //
3 // File: model_fiducial.c
4 // Author: Richard Vaughan
5 // Date: 10 June 2004
6 //
7 // CVS info:
8 // $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_fiducial.cc,v $
9 // $Author: rtv $
314e392 fixed rotation of model_actuator, thanks to patch #2927791 from RAZOR
rtv authored
10 // $Revision$
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
11 //
12 ///////////////////////////////////////////////////////////////////////////
13
5c7470d beautifying fasr2 code
rtv authored
14 #undef DEBUG
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
15
ec56d04 header clean up
rtv authored
16 #include "stage.hh"
17 #include "option.hh"
18 #include "worldfile.hh"
19 using namespace Stg;
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
20
e2df9ad started to fix bug when changes to data filter doesnt redraw screen - fi...
alexcb authored
21 //TODO make instance attempt to register an option (as customvisualizations do)
850944c tweaked fiducial vis. working on lights
rtv authored
22 Option ModelFiducial::showData( "Fiducials", "show_fiducial", "", true, NULL );
23 Option ModelFiducial::showFov( "Fiducial FOV", "show_fiducial_fov", "", false, NULL );
e896614 Added filter options for all sensor classes
jeremy_asher authored
24
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
25 /**
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
26 @ingroup model
27 @defgroup model_fiducial Fiducial detector model
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
28
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
29 The fiducial model simulates a fiducial-detecting device.
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
30
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
31 API: Stg::ModelFiducial
1a59282 major revisions to documentation, which was very out of date
rtv authored
32
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
33 <h2>Worldfile properties</h2>
34
35 @par Summary and default values
36
37 @verbatim
e2df9ad started to fix bug when changes to data filter doesnt redraw screen - fi...
alexcb authored
38 fiducial
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
39 (
e2df9ad started to fix bug when changes to data filter doesnt redraw screen - fi...
alexcb authored
40 # fiducial properties
8f51e10 Worldfile documentation updates
jeremy_asher authored
41 range_min 0.0
42 range_max 8.0
43 range_max_id 5.0
44 fov 3.14159
d32d047 unbroke fiducial. working on fasr2
rtv authored
45 ignore_zloc 0
8f51e10 Worldfile documentation updates
jeremy_asher authored
46
47 # model properties
48 size [ 0.1 0.1 0.1 ]
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
49 )
50 @endverbatim
51
52 @par Details
7f76edc Spacing fix in docs
jeremy_asher authored
53
8f51e10 Worldfile documentation updates
jeremy_asher authored
54 - range_min <float>\n
55 the minimum range reported by the sensor, in meters. The sensor will detect objects closer than this, but report their range as the minimum.
56 - range_max <float>\n
57 the maximum range at which the sensor can detect a fiducial, in meters. The sensor may not be able to uinquely identify the fiducial, depending on the value of range_max_id.
58 - range_max_id <float>\n
59 the maximum range at which the sensor can detect the ID of a fiducial, in meters.
60 - fov <float>
61 the angular field of view of the scanner, in radians.
ad6baa4 added option to ignore fiducial Z location, inspired by patch #2886830 b...
rtv authored
62 - ignore_zloc <1/0>\n
d32d047 unbroke fiducial. working on fasr2
rtv authored
63 default is 0. When set to 1, the fiducial finder ignores the z component when checking a fiducial. Using the default behaviour, a short object would not been seen
64 by a fiducial finder placed on top of a tall robot. With this flag set to 1, the fiducial finder will see the shorter robot.
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
65 */
850944c tweaked fiducial vis. working on lights
rtv authored
66
67 ModelFiducial::ModelFiducial( World* world,
5a63ae0 removed toby's over-specific models and player interfaces. simplified mo...
rtv authored
68 Model* parent,
69 const std::string& type ) :
70 Model( world, parent, type ),
850944c tweaked fiducial vis. working on lights
rtv authored
71 fiducials(),
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
72 max_range_anon( 8.0 ),
73 max_range_id( 5.0 ),
74 min_range( 0.0 ),
75 fov( M_PI ),
850944c tweaked fiducial vis. working on lights
rtv authored
76 heading( 0 ),
ad6baa4 added option to ignore fiducial Z location, inspired by patch #2886830 b...
rtv authored
77 key( 0 ),
78 ignore_zloc(false)
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
79 {
850944c tweaked fiducial vis. working on lights
rtv authored
80 //PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n",
81 // id, typestr );
82
83 // assert that Update() is reentrant for this derived model
84 thread_safe = true;
85
86 // sensible fiducial defaults
87 // interval = 200; // common for a SICK LMS200
88
89 this->ClearBlocks();
90
91 Geom geom;
92 geom.Zero();
93 SetGeom( geom );
94
95 RegisterOption( &showData );
96 RegisterOption( &showFov );
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
97 }
98
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
99 ModelFiducial::~ModelFiducial( void )
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
100 {
101 }
102
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
103 static bool fiducial_raytrace_match( Model* candidate,
104 Model* finder,
9621f09 major changes for 3.1
rtv authored
105 const void* dummy )
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
106 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
107 (void)dummy; // avoid warning about unused var
9621f09 major changes for 3.1
rtv authored
108 return( ! finder->IsRelated( candidate ) );
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
109 }
110
111
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
112 void ModelFiducial::AddModelIfVisible( Model* him )
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
113 {
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
114 //PRINT_DEBUG2( "Fiducial %s is testing model %s", token, him->Token() );
115
d32d047 unbroke fiducial. working on fasr2
rtv authored
116 // only non-zero IDs should ever be checked
117 assert( him->vis.fiducial_return != 0 );
118
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
119 // check to see if this neighbor has the right fiducial key
d32d047 unbroke fiducial. working on fasr2
rtv authored
120 // if keys are used extensively, then perhaps a vector per key would be faster
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
121 if( vis.fiducial_key != him->vis.fiducial_key )
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
122 {
123 //PRINT_DEBUG1( " but model %s doesn't match the fiducial key", him->Token());
124 return;
125 }
126
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
127 Pose mypose = this->GetGlobalPose();
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
128
129 // are we within range?
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
130 Pose hispose = him->GetGlobalPose();
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
131 double dx = hispose.x - mypose.x;
132 double dy = hispose.y - mypose.y;
133 double range = hypot( dy, dx );
134
135 // printf( "range to target %.2f m (
136
d32d047 unbroke fiducial. working on fasr2
rtv authored
137 if( range >= max_range_anon )
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
138 {
139 //PRINT_DEBUG3( " but model %s is %.2f m away, outside my range of %.2f m",
140 // him->Token(),
141 // range,
142 // max_range_anon );
143 return;
144 }
145
146 // is he in my field of view?
147 double bearing = atan2( dy, dx );
148 double dtheta = normalize(bearing - mypose.a);
149
150 if( fabs(dtheta) > fov/2.0 )
151 {
152 //PRINT_DEBUG1( " but model %s is outside my FOV", him->Token());
153 return;
154 }
155
156 if( IsRelated( him ) )
157 return;
158
6e1afc1 fasr2 demo with basic recharging
rtv authored
159 //PRINT_DEBUG1( " %s is a candidate. doing ray trace", him->Token());
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
160
161
162 //printf( "bearing %.2f\n", RTOD(bearing) );
d32d047 unbroke fiducial. working on fasr2
rtv authored
163
164 //If we've gotten to this point, a few things are true:
165 // 1. The fiducial is in the field of view of the finder.
166 // 2. The fiducial is in range of the finder.
167 //At this point the purpose of the ray trace is to start at the finder and see if there is anything
168 //in between the finder and the fiducial. If we simply trace out to the distance we know the finder
169 //is at, then the resulting ray.mod can be one of three things:
170 // 1. A pointer to the model we're tracing to. In this case the model is at the right Zloc to be
171 // returned by the ray tracer.
172 // 2. A pointer to another model that blocked the ray.
173 // 3. NULL. If it's null, then it means that the ray traced to where the fiducial should be but
174 // it's zloc was such that the ray didn't hit it. However, we DO know its there, so we can
175 // return this as a hit.
176
177 //printf( "range %.2f\n", range );
178
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
179 RaytraceResult ray( Raytrace( dtheta,
d32d047 unbroke fiducial. working on fasr2
rtv authored
180 max_range_anon, // TODOscan only as far as the object
181 fiducial_raytrace_match,
182 NULL,
183 true ) );
9621f09 major changes for 3.1
rtv authored
184
d32d047 unbroke fiducial. working on fasr2
rtv authored
185 // TODO
186 //if( ignore_zloc && ray.mod == NULL ) // i.e. we didn't hit anything *else*
187 //ray.mod = him; // so he was just at the wrong height
188
189 //printf( "ray hit %s and was seeking LOS to %s\n",
190 // ray.mod ? ray.mod->Token() : "null",
191 // him->Token() );
6e1afc1 fasr2 demo with basic recharging
rtv authored
192
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
193 // if it was him, we can see him
d32d047 unbroke fiducial. working on fasr2
rtv authored
194 if( ray.mod != him )
195 return;
196
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
197 assert( range >= 0 );
198
199 // passed all the tests! record the fiducial hit
200
201 Geom hisgeom( him->GetGeom() );
202
203 // record where we saw him and what he looked like
204 Fiducial fid;
205 fid.mod = him;
206 fid.range = range;
207 fid.bearing = dtheta;
208 fid.geom.x = hisgeom.size.x;
209 fid.geom.y = hisgeom.size.y;
210 fid.geom.a = normalize( hispose.a - mypose.a);
211
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
212 //fid.pose_rel = hispose - this->GetGlobalPose();
213
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
214 // store the global pose of the fiducial (mainly for the GUI)
215 fid.pose = hispose;
216
217 // if he's within ID range, get his fiducial.return value, else
218 // we see value 0
ad6baa4 added option to ignore fiducial Z location, inspired by patch #2886830 b...
rtv authored
219 fid.id = range < max_range_id ? him->vis.fiducial_return : 0;
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
220
6e1afc1 fasr2 demo with basic recharging
rtv authored
221 //PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
222 // him->Token(), him->vis.fiducial_return );
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
223
224 fiducials.push_back( fid );
225 }
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
226
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
227
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
228 ///////////////////////////////////////////////////////////////////////////
229 // Update the beacon data
230 //
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
231 void ModelFiducial::Update( void )
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
232 {
6e1afc1 fasr2 demo with basic recharging
rtv authored
233 //PRINT_DEBUG( "fiducial update" );
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
234
235 if( subs < 1 )
236 return;
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
237
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
238 // reset the array of detected fiducials
a7f58e5 API clean up and small speed-ups
rtv authored
239 fiducials.clear();
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
240
241 #if( 1 )
242 // BEGIN EXPERIMENT
243
244 // find two sets of fiducial-bearing models, within sensor range on
245 // the two different axes
246
247 double rng = max_range_anon;
248 Pose gp = GetGlobalPose();
249 Model edge; // dummy model used to find bounds in the sets
250
251 edge.pose = Pose( gp.x-rng, gp.y, 0, 0 ); // LEFT
252 std::set<Model*,World::ltx>::iterator xmin =
b45dd80 removed FLTK dependencies from stage.pc
Richard Vaughan authored
253 world->models_with_fiducials_byx.lower_bound( &edge ); // O(log(n))
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
254
255 edge.pose = Pose( gp.x+rng, gp.y, 0, 0 ); // RIGHT
256 const std::set<Model*,World::ltx>::iterator xmax =
257 world->models_with_fiducials_byx.upper_bound( &edge );
258
259 edge.pose = Pose( gp.x, gp.y-rng, 0, 0 ); // BOTTOM
260 std::set<Model*,World::lty>::iterator ymin =
261 world->models_with_fiducials_byy.lower_bound( &edge );
262
263 edge.pose = Pose( gp.x, gp.y+rng, 0, 0 ); // TOP
264 const std::set<Model*,World::lty>::iterator ymax =
265 world->models_with_fiducials_byy.upper_bound( &edge );
266
267 // put these models into sets keyed on model pointer, rather than position
268 std::set<Model*> horiz, vert;
269
270 for( ; xmin != xmax; xmin++)
271 horiz.insert( *xmin);
272
273 for( ; ymin != ymax; ymin++ )
274 vert.insert( *ymin );
275
b45dd80 removed FLTK dependencies from stage.pc
Richard Vaughan authored
276 // the intersection of the sets is all the fiducials close by
277 std::vector<Model*> nearby;
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
278 std::set_intersection( horiz.begin(), horiz.end(),
279 vert.begin(), vert.end(),
b45dd80 removed FLTK dependencies from stage.pc
Richard Vaughan authored
280 std::inserter( nearby, nearby.end() ) );
a7f58e5 API clean up and small speed-ups
rtv authored
281
b45dd80 removed FLTK dependencies from stage.pc
Richard Vaughan authored
282 // printf( "cand sz %lu\n", nearby.size() );
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
283
284 // create sets sorted by x and y position
b45dd80 removed FLTK dependencies from stage.pc
Richard Vaughan authored
285 FOR_EACH( it, nearby )
649ba76 O(n log n) fiducial finder
Richard Vaughan authored
286 AddModelIfVisible( *it );
287 #else
288 // create sets sorted by x and y position
289 FOR_EACH( it, world->models_with_fiducials )
290 AddModelIfVisible( *it );
291
292 #endif
293
294 // find the range of fiducials within range in X
7664094 fixed bug in gripper related to subs/unsubs
rtv authored
295
296 Model::Update();
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
297 }
298
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
299 void ModelFiducial::Load( void )
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
300 {
301 PRINT_DEBUG( "fiducial load" );
302
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
303 Model::Load();
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
304
305 // load fiducial-specific properties
ad6baa4 added option to ignore fiducial Z location, inspired by patch #2886830 b...
rtv authored
306 min_range = wf->ReadLength( wf_entity, "range_min", min_range );
307 max_range_anon = wf->ReadLength( wf_entity, "range_max", max_range_anon );
308 max_range_id = wf->ReadLength( wf_entity, "range_max_id", max_range_id );
309 fov = wf->ReadAngle ( wf_entity, "fov", fov );
5c7470d beautifying fasr2 code
rtv authored
310 ignore_zloc = wf->ReadInt ( wf_entity, "ignore_zloc", ignore_zloc);
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
311 }
312
313
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
314 void ModelFiducial::DataVisualize( Camera* cam )
22a1279 fixed indentation issues (converted to tabs)
alexcb authored
315 {
3ce2d0e applied patch from Jeff Donner, fixing some signed/unsigned comparisons
rtv authored
316 (void)cam; // avoid warning about unused var
317
850944c tweaked fiducial vis. working on lights
rtv authored
318 if( showFov )
319 {
320 PushColor( 1,0,1,0.2 ); // magenta, with a bit of alpha
a7f58e5 API clean up and small speed-ups
rtv authored
321
850944c tweaked fiducial vis. working on lights
rtv authored
322 GLUquadric* quadric = gluNewQuadric();
323
324 gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
325
326 gluPartialDisk( quadric,
327 0,
328 max_range_anon,
329 20, // slices
330 1, // loops
331 rtod( M_PI/2.0 + fov/2.0), // start angle
332 rtod(-fov) ); // sweep angle
333
334 gluDeleteQuadric( quadric );
335
336 PopColor();
337 }
a7f58e5 API clean up and small speed-ups
rtv authored
338
850944c tweaked fiducial vis. working on lights
rtv authored
339 if( showData )
340 {
341 PushColor( 1,0,1,0.4 ); // magenta, with a bit of alpha
342
343 // draw fuzzy dotted lines
344 glLineWidth( 2.0 );
345 glLineStipple( 1, 0x00FF );
346
347 // draw lines to the fiducials
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
348 FOR_EACH( it, fiducials )
850944c tweaked fiducial vis. working on lights
rtv authored
349 {
f5bbd05 fixed event queuing bug when unsubscribing & subscribing
rtv authored
350 Fiducial& fid = *it;
850944c tweaked fiducial vis. working on lights
rtv authored
351
352 double dx = fid.range * cos( fid.bearing);
353 double dy = fid.range * sin( fid.bearing);
354
355 glEnable(GL_LINE_STIPPLE);
356 glBegin( GL_LINES );
357 glVertex2f( 0,0 );
358 glVertex2f( dx, dy );
359 glEnd();
360 glDisable(GL_LINE_STIPPLE);
361
362 glPushMatrix();
363 Gl::coord_shift( dx,dy,0,fid.geom.a );
364
365 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
366 glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
367 fid.geom.x/2.0, fid.geom.y/2.0 );
368
369 // show the fiducial ID
370 char idstr[32];
371 snprintf(idstr, 31, "%d", fid.id );
372 Gl::draw_string( 0,0,0, idstr );
373
374 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
375 glPopMatrix();
376 }
377
378 PopColor();
379 glLineWidth( 1.0 );
380 }
36bfbdb first commit since shifting the Stage-3.0 development branch (opengl) to...
rtv authored
381 }
a7f58e5 API clean up and small speed-ups
rtv authored
382
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
383 void ModelFiducial::Shutdown( void )
906adda stage: fixed ForEachModel being called multiple times for some models by...
jeremy_asher authored
384 {
f7b9355 fasr2
rtv authored
385 //PRINT_DEBUG( "fiducial shutdown" );
a7f58e5 API clean up and small speed-ups
rtv authored
386 fiducials.clear();
3680786 API changes! removed the Stg prefix from all class names, and renamed so...
rtv authored
387 Model::Shutdown();
906adda stage: fixed ForEachModel being called multiple times for some models by...
jeremy_asher authored
388 }
Something went wrong with that request. Please try again.