Skip to content
This repository
branch: accel
Fetching contributors…

Cannot retrieve contributors at this time

file 388 lines (297 sloc) 10.74 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388
///////////////////////////////////////////////////////////////////////////
//
// File: model_fiducial.c
// Author: Richard Vaughan
// Date: 10 June 2004
//
// CVS info:
// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_fiducial.cc,v $
// $Author: rtv $
// $Revision$
//
///////////////////////////////////////////////////////////////////////////

#undef DEBUG

#include "stage.hh"
#include "option.hh"
#include "worldfile.hh"
using namespace Stg;

//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelFiducial::showData( "Fiducials", "show_fiducial", "", true, NULL );
Option ModelFiducial::showFov( "Fiducial FOV", "show_fiducial_fov", "", false, NULL );

/**
@ingroup model
@defgroup model_fiducial Fiducial detector model

The fiducial model simulates a fiducial-detecting device.

API: Stg::ModelFiducial

<h2>Worldfile properties</h2>

@par Summary and default values

@verbatim
fiducial
(
# fiducial properties
range_min 0.0
range_max 8.0
range_max_id 5.0
fov 3.14159
ignore_zloc 0

# model properties
size [ 0.1 0.1 0.1 ]
)
@endverbatim

@par Details
- range_min <float>\n
the minimum range reported by the sensor, in meters. The sensor will detect objects closer than this, but report their range as the minimum.
- range_max <float>\n
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.
- range_max_id <float>\n
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 object would not been seen
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.
*/
  
  ModelFiducial::ModelFiducial( World* world,
Model* parent,
const std::string& type ) :
  Model( world, parent, type ),
  fiducials(),
  max_range_anon( 8.0 ),
  max_range_id( 5.0 ),
  min_range( 0.0 ),
  fov( M_PI ),
  heading( 0 ),
  key( 0 ),
  ignore_zloc(false)
{
  //PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n",
  // id, typestr );
  
  // assert that Update() is reentrant for this derived model
  thread_safe = true;
  
  // sensible fiducial defaults
  // interval = 200; // common for a SICK LMS200
  
  this->ClearBlocks();
  
  Geom geom;
  geom.Zero();
  SetGeom( geom );
  
  RegisterOption( &showData );
  RegisterOption( &showFov );
}

ModelFiducial::~ModelFiducial( void )
{
}

static bool fiducial_raytrace_match( Model* candidate,
Model* finder,
const void* dummy )
{
  (void)dummy; // avoid warning about unused var
  return( ! finder->IsRelated( candidate ) );
}


void ModelFiducial::AddModelIfVisible( Model* him )
{
//PRINT_DEBUG2( "Fiducial %s is testing model %s", token, him->Token() );

// only non-zero IDs should ever be checked
assert( him->vis.fiducial_return != 0 );

// check to see if this neighbor has the right fiducial key
// if keys are used extensively, then perhaps a vector per key would be faster
if( vis.fiducial_key != him->vis.fiducial_key )
{
//PRINT_DEBUG1( " but model %s doesn't match the fiducial key", him->Token());
return;
}

Pose mypose = this->GetGlobalPose();

// are we within range?
Pose hispose = him->GetGlobalPose();
double dx = hispose.x - mypose.x;
double dy = hispose.y - mypose.y;
double range = hypot( dy, dx );

// printf( "range to target %.2f m (

if( range >= max_range_anon )
{
//PRINT_DEBUG3( " but model %s is %.2f m away, outside my range of %.2f m",
// him->Token(),
// range,
// max_range_anon );
return;
}

// is he in my field of view?
double bearing = atan2( dy, dx );
double dtheta = normalize(bearing - mypose.a);

if( fabs(dtheta) > fov/2.0 )
{
//PRINT_DEBUG1( " but model %s is outside my FOV", him->Token());
return;
}

if( IsRelated( him ) )
return;

//PRINT_DEBUG1( " %s is a candidate. doing ray trace", him->Token());


//printf( "bearing %.2f\n", RTOD(bearing) );

//If we've gotten to this point, a few things are true:
// 1. The fiducial is in the field of view of the finder.
// 2. The fiducial is in range of the finder.
//At this point the purpose of the ray trace is to start at the finder and see if there is anything
//in between the finder and the fiducial. If we simply trace out to the distance we know the finder
//is at, then the resulting ray.mod can be one of three things:
// 1. A pointer to the model we're tracing to. In this case the model is at the right Zloc to be
// returned by the ray tracer.
// 2. A pointer to another model that blocked the ray.
// 3. NULL. If it's null, then it means that the ray traced to where the fiducial should be but
// it's zloc was such that the ray didn't hit it. However, we DO know its there, so we can
// return this as a hit.

//printf( "range %.2f\n", range );

RaytraceResult ray( Raytrace( dtheta,
max_range_anon, // TODOscan only as far as the object
fiducial_raytrace_match,
NULL,
true ) );

// TODO
if( ignore_zloc && ray.mod == NULL ) // i.e. we didn't hit anything *else*
ray.mod = him; // so he was just at the wrong height

//printf( "ray hit %s and was seeking LOS to %s\n",
// ray.mod ? ray.mod->Token() : "null",
// him->Token() );

// if it was him, we can see him
if( ray.mod != him )
return;

assert( range >= 0 );

// passed all the tests! record the fiducial hit

Geom hisgeom( him->GetGeom() );

// record where we saw him and what he looked like
Fiducial fid;
fid.mod = him;
fid.range = range;
fid.bearing = dtheta;
fid.geom.x = hisgeom.size.x;
fid.geom.y = hisgeom.size.y;
fid.geom.z = hisgeom.size.z;
fid.geom.a = normalize( hispose.a - mypose.a);

//fid.pose_rel = hispose - this->GetGlobalPose();

// store the global pose of the fiducial (mainly for the GUI)
fid.pose = hispose;

// if he's within ID range, get his fiducial.return value, else
// we see value 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 );

fiducials.push_back( fid );
}


///////////////////////////////////////////////////////////////////////////
// Update the beacon data
//
void ModelFiducial::Update( void )
{
  //PRINT_DEBUG( "fiducial update" );

if( subs < 1 )
return;

// reset the array of detected fiducials
fiducials.clear();

#if( 1 )
// BEGIN EXPERIMENT

// find two sets of fiducial-bearing models, within sensor range on
// the two different axes

double rng = max_range_anon;
Pose gp = GetGlobalPose();
Model edge; // dummy model used to find bounds in the sets

edge.pose = Pose( gp.x-rng, gp.y, 0, 0 ); // LEFT
std::set<Model*,World::ltx>::iterator xmin =
world->models_with_fiducials_byx.lower_bound( &edge ); // O(log(n))

edge.pose = Pose( gp.x+rng, gp.y, 0, 0 ); // RIGHT
const std::set<Model*,World::ltx>::iterator xmax =
world->models_with_fiducials_byx.upper_bound( &edge );

edge.pose = Pose( gp.x, gp.y-rng, 0, 0 ); // BOTTOM
std::set<Model*,World::lty>::iterator ymin =
world->models_with_fiducials_byy.lower_bound( &edge );

edge.pose = Pose( gp.x, gp.y+rng, 0, 0 ); // TOP
const std::set<Model*,World::lty>::iterator ymax =
world->models_with_fiducials_byy.upper_bound( &edge );

// put these models into sets keyed on model pointer, rather than position
std::set<Model*> horiz, vert;

for( ; xmin != xmax; ++xmin)
horiz.insert( *xmin);

for( ; ymin != ymax; ++ymin )
vert.insert( *ymin );

// the intersection of the sets is all the fiducials close by
std::vector<Model*> nearby;
std::set_intersection( horiz.begin(), horiz.end(),
vert.begin(), vert.end(),
std::inserter( nearby, nearby.end() ) );

// printf( "cand sz %lu\n", nearby.size() );

// create sets sorted by x and y position
  FOR_EACH( it, nearby )
  AddModelIfVisible( *it );
#else
  FOR_EACH( it, world->models_with_fiducials )
  AddModelIfVisible( *it );

#endif

// find the range of fiducials within range in X

Model::Update();
}

void ModelFiducial::Load( void )
{
PRINT_DEBUG( "fiducial load" );

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 );
  ignore_zloc = wf->ReadInt ( wf_entity, "ignore_zloc", ignore_zloc);
}


void ModelFiducial::DataVisualize( Camera* cam )
{
  (void)cam; // avoid warning about unused var

if( showFov )
{
PushColor( 1,0,1,0.2 ); // magenta, with a bit of alpha

GLUquadric* quadric = gluNewQuadric();

gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );

gluPartialDisk( quadric,
0,
max_range_anon,
20, // slices
1, // loops
rtod( M_PI/2.0 + fov/2.0), // start angle
rtod(-fov) ); // sweep angle

gluDeleteQuadric( quadric );

PopColor();
}

if( showData )
{
PushColor( 1,0,1,0.4 ); // magenta, with a bit of alpha

// draw fuzzy dotted lines
glLineWidth( 2.0 );
glLineStipple( 1, 0x00FF );

// draw lines to the fiducials
FOR_EACH( it, fiducials )
{
Fiducial& fid = *it;

double dx = fid.range * cos( fid.bearing);
double dy = fid.range * sin( fid.bearing);

glEnable(GL_LINE_STIPPLE);
glBegin( GL_LINES );
glVertex2f( 0,0 );
glVertex2f( dx, dy );
glEnd();
glDisable(GL_LINE_STIPPLE);

glPushMatrix();
Gl::coord_shift( dx,dy,0,fid.geom.a );

glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
fid.geom.x/2.0, fid.geom.y/2.0 );

// show the fiducial ID
char idstr[32];
snprintf(idstr, 31, "%d", fid.id );
Gl::draw_string( 0,0,0, idstr );

glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glPopMatrix();
}

PopColor();
glLineWidth( 1.0 );
}
}

void ModelFiducial::Shutdown( void )
{
  //PRINT_DEBUG( "fiducial shutdown" );
fiducials.clear();
Model::Shutdown();
}
Something went wrong with that request. Please try again.