model_ranger.cc
Go to the documentation of this file.
1 //
3 // File: model_ranger.cc
4 // Author: Richard Vaughan
5 // Date: 10 June 2004
6 //
7 // CVS info:
8 // $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_ranger.cc,v $
9 // $Author: rtv $
10 // $Revision$
11 //
13 
65 //#define DEBUG 1
66 
67 #include "stage.hh"
68 #include "worldfile.hh"
69 #include "option.hh"
70 using namespace Stg;
71 
72 static const watts_t RANGER_WATTSPERSENSOR = 0.2;
73 static const Stg::Size RANGER_SIZE( 0.15, 0.15, 0.2 ); // SICK LMS size
74 
75 //static const Color RANGER_COLOR( 0,0,1 );
76 static const Color RANGER_CONFIG_COLOR( 0,0,0.5 );
77 //static const Color RANGER_GEOM_COLOR( 1,0,1 );
78 
79 // static members
80 Option ModelRanger::Vis::showTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL );
81 Option ModelRanger::Vis::showArea( "Ranger area", "show_ranger_ranges", "", true, NULL );
82 Option ModelRanger::Vis::showStrikes( "Ranger strikes", "show_ranger_strikes", "", false, NULL );
83 Option ModelRanger::Vis::showFov( "Ranger FOV", "show_ranger_fov", "", false, NULL );
84 Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "", false, NULL );
85 
86 
88  Model* parent,
89  const std::string& type )
90  : Model( world, parent, type ),
91  vis( world )
92 {
93  PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
94  id, type );
95 
96  // Set up sensible defaults
97 
98  // assert that Update() is reentrant for this derived model
99  thread_safe = true;
100 
101  this->SetColor( RANGER_CONFIG_COLOR );
102 
103  // remove the polygon: ranger has no body
104  this->ClearBlocks();
105 
106  this->SetGeom( Geom( Pose(), RANGER_SIZE ));
107 
108  AddVisualizer( &vis, true );
109 }
110 
112 {
113 }
114 
116 {
117  Model::Startup();
118  this->SetWatts( RANGER_WATTSPERSENSOR * sensors.size() );
119 }
120 
121 
123 {
124  PRINT_DEBUG( "ranger shutdown" );
125 
126  this->SetWatts( 0 );
127 
128  Model::Shutdown();
129 }
130 
132 {
133  //static int c=0;
134  // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ );
135  Sensor s;
136  s.Load( wf, entity );
137  sensors.push_back(s);
138 }
139 
140 
142 {
143  //static int c=0;
144  // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ );
145 
146  pose.Load( wf, entity, "pose" );
147  size.Load( wf, entity, "size" );
148  range.Load( wf, entity, "range" );
149  col.Load( wf, entity );
150  fov = wf->ReadAngle( entity, "fov", fov );
151  sample_count = wf->ReadInt( entity, "samples", sample_count );
152  //ranges.resize(sample_count);
153  //intensities.resize(sample_count);
154 }
155 
156 void ModelRanger::Load( void )
157 {
158  Model::Load();
159 }
160 
161 static bool ranger_match( Model* hit,
162  Model* finder,
163  const void* dummy )
164 {
165  (void)dummy; // avoid warning about unused var
166 
167  // Ignore the model that's looking and things that are invisible to
168  // rangers
169 
170  // small optimization to avoid recursive Model::IsRelated call in common cases
171  if( (hit == finder->Parent()) || (hit == finder) ) return false;
172 
173  return( (!hit->IsRelated( finder )) && (sgn(hit->vis.ranger_return) != -1 ) );
174 }
175 
177 {
178  // raytrace new range data for all sensors
179  FOR_EACH( it, sensors )
180  it->Update( this );
181 
182  Model::Update();
183 }
184 
186 {
187  ranges.resize( sample_count );
188  intensities.resize( sample_count );
189  bearings.resize( sample_count );
190 
191  //printf( "update sensor, has ranges size %u\n", (unsigned int)ranges.size() );
192  // make the first and last rays exactly at the extremes of the FOV
193  double sample_incr( fov / std::max(sample_count-1, (unsigned int)1) );
194 
195  // find the global origin of our first emmitted ray
196  double start_angle = (sample_count > 1 ? -fov/2.0 : 0.0);
197 
198  Pose rayorg(pose);
199  rayorg.a += start_angle;
200  rayorg.z += size.z/2.0;
201  rayorg = mod->LocalToGlobal(rayorg);
202 
203  // set up a ray to trace
204  Ray ray( mod, rayorg, range.max, ranger_match, NULL, true );
205 
206  World* world = mod->GetWorld();
207 
208  // trace the ray, incrementing its heading for each sample
209  for( size_t t(0); t<sample_count; t++ )
210  {
211  const RaytraceResult& r ( world->Raytrace( ray ) );
212  ranges[t] = r.range;
213  intensities[t] = r.mod ? r.mod->vis.ranger_return : 0.0;
214  bearings[t] = start_angle + ((double)t) * sample_incr;
215 
216  // point the ray to the next angle
217  ray.origin.a += sample_incr;
218 
219  //printf( "ranger %s sensor %p pose %s sample %d range %.2f ref %.2f\n",
220  // mod->Token(),
221  // this,
222  // pose.String().c_str(),
223  // t,
224  // ranges[t].range,
225  // ranges[t].reflectance );
226  }
227 }
228 
229 std::string ModelRanger::Sensor::String() const
230 {
231  char buf[256];
232  snprintf( buf, 256, "[ samples %u, range [%.2f %.2f] ]",
233  sample_count, range.min, range.max );
234  return( std::string( buf ) );
235 }
236 
238 {
239  size_t sample_count( this->sample_count );
240 
241  //glTranslatef( 0,0, ranger->GetGeom().size.z/2.0 ); // shoot the ranger beam out at the right height
242 
243  // pack the ranger hit points into a vertex array for fast rendering
244  GLfloat pts[2*(sample_count+1)];
245  glVertexPointer( 2, GL_FLOAT, 0, &pts[0] );
246 
247  pts[0] = 0.0;
248  pts[1] = 0.0;
249 
250  glDepthMask( GL_FALSE );
251  glPointSize( 2 );
252 
253  glPushMatrix();
254 
255  Gl::pose_shift( pose );
256 
257  if( vis->showTransducers )
258  {
259  // draw the sensor body as a rectangle
260  rgr->PushColor( col );
261  glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
262  glRectf( -size.x/2.0, -size.y/2.0, size.x/2.0, size.y/2.0 );
263  rgr->PopColor();
264  }
265 
266  Color c( col );
267  c.a = 0.15; // transparent version of sensor color
268  rgr->PushColor( c );
269  glPolygonMode( GL_FRONT, GL_FILL );
270 
271  if( ranges.size() ) // if we have some data
272  {
273  if( sample_count == 1 )
274  {
275  // only one sample, so we fake up some beam width for beauty
276  const double sidelen = ranges[0];
277  const double da = fov/2.0;
278 
279  sample_count = 3;
280 
281  pts[2] = sidelen*cos(-da );
282  pts[3] = sidelen*sin(-da );
283 
284  pts[4] = sidelen*cos(+da );
285  pts[5] = sidelen*sin(+da );
286  }
287  else
288  {
289  for( size_t s(0); s<sample_count+1; s++ )
290  {
291  double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0;
292  pts[2*s+2] = (float)(ranges[s] * cos(ray_angle) );
293  pts[2*s+3] = (float)(ranges[s] * sin(ray_angle) );
294  }
295  }
296  }
297 
298  if( vis->showArea )
299  {
300  if( sample_count > 1 )
301  // draw the filled polygon in transparent blue
302  glDrawArrays( GL_POLYGON, 0, sample_count );
303  }
304 
305  glDepthMask( GL_TRUE );
306 
307  if( vis->showStrikes )
308  {
309  // TODO - paint the stike point in a color based on intensity
310  // // if the sample is unusually bright, draw a little blob
311  // if( intensities[s] > 0.0 )
312  // {
313  // // this happens rarely so we can do it in immediate mode
314  // glBegin( GL_POINTS );
315  // glVertex2f( pts[2*s+2], pts[2*s+3] );
316  // glEnd();
317  // }
318 
319  // draw the beam strike points
320  c.a = 0.8;
321  rgr->PushColor( c );
322  glDrawArrays( GL_POINTS, 0, sample_count+1 );
323  rgr->PopColor();
324  }
325 
326  if( vis->showFov )
327  {
328  for( size_t s(0); s<sample_count; s++ )
329  {
330  double ray_angle((s * (fov / (sample_count-1))) - fov/2.0);
331  pts[2*s+2] = (float)(range.max * cos(ray_angle) );
332  pts[2*s+3] = (float)(range.max * sin(ray_angle) );
333  }
334 
335  glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
336  c.a = 0.5;
337  rgr->PushColor( c );
338  glDrawArrays( GL_POLYGON, 0, sample_count+1 );
339  rgr->PopColor();
340  }
341 
342  if( vis->showBeams )
343  {
344  // darker version of the same color
345  c.r /= 2.0;
346  c.g /= 2.0;
347  c.b /= 2.0;
348  c.a = 1.0;
349 
350  rgr->PushColor( c );
351  glBegin( GL_LINES );
352 
353  for( size_t s(0); s<sample_count; s++ )
354  {
355 
356  glVertex2f( 0,0 );
357  double ray_angle( sample_count == 1 ? 0 : (s * (fov / (sample_count-1))) - fov/2.0 );
358  glVertex2f( ranges[s] * cos(ray_angle),
359  ranges[s] * sin(ray_angle) );
360 
361  }
362  glEnd();
363  rgr->PopColor();
364  }
365 
366  rgr->PopColor();
367 
368  glPopMatrix();
369 }
370 
371 void ModelRanger::Print( char* prefix ) const
372 {
373  Model::Print( prefix );
374 
375  printf( "\tRanges " );
376  for( size_t i(0); i<sensors.size(); i++ )
377  {
378  printf( "[ " );
379  for( size_t j(0); j<sensors[i].ranges.size(); j++ )
380  printf( "%.2f ", sensors[i].ranges[j] );
381 
382  printf( " ]" );
383  }
384 
385  printf( "\n\tIntensities " );
386  for( size_t i(0); i<sensors.size(); i++ )
387  {
388  printf( "[ " );
389  for( size_t j(0); j<sensors[i].intensities.size(); j++ )
390  printf( "%.2f ", sensors[i].intensities[j] );
391 
392  printf( " ]" );
393  }
394  puts("");
395 }
396 
397 
398 
399 // VIS -------------------------------------------------------------------
400 
402  : Visualizer( "Ranger", "ranger_vis" )
403 {
404  world->RegisterOption( &showArea );
405  world->RegisterOption( &showStrikes );
406  world->RegisterOption( &showFov );
407  world->RegisterOption( &showBeams );
408  world->RegisterOption( &showTransducers );
409 }
410 
412 {
413  (void)cam; // avoid warning about unused var
414 
415  ModelRanger* ranger( dynamic_cast<ModelRanger*>(mod) );
416 
417  const std::vector<Sensor>& sensors( ranger->GetSensors() );
418 
419  FOR_EACH( it, sensors )
420  it->Visualize( this, ranger );
421 
422  const size_t sensor_count = sensors.size();
423 
424  if( showTransducers )
425  {
426  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
427  ranger->PushColor( 0,0,0,1 );
428 
429  for( size_t s(0); s<sensor_count; s++ )
430  {
431  const Sensor& rngr(sensors[s]);
432 
433  glPointSize( 4 );
434  glBegin( GL_POINTS );
435  glVertex3f( rngr.pose.x, rngr.pose.y, rngr.pose.z );
436  glEnd();
437 
438  char buf[8];
439  snprintf( buf, 8, "%d", (int)s );
440  Gl::draw_string( rngr.pose.x, rngr.pose.y, rngr.pose.z, buf );
441 
442  }
443  ranger->PopColor();
444  }
445 
446 }
447 
virtual void PushColor(Color col)
Definition: stage.hh:2189
Model class
Definition: stage.hh:1742
static const watts_t RANGER_WATTSPERSENSOR
Definition: model_ranger.cc:72
Model * Parent() const
Definition: stage.hh:2299
static const Color RANGER_CONFIG_COLOR(0, 0, 0.5)
double a
Definition: stage.hh:200
ModelRanger(World *world, Model *parent, const std::string &type)
Definition: model_ranger.cc:87
World class
Definition: stage.hh:814
The Stage library uses its own namespace.
Definition: canvas.hh:8
Stg::ModelRanger::Vis vis
std::string String() const
void RegisterOption(Option *opt)
Register an Option for pickup by the GUI.
Definition: world.cc:1160
static const Stg::Size RANGER_SIZE(0.15, 0.15, 0.2)
#define PRINT_DEBUG2(m, a, b)
Definition: stage.hh:668
virtual void Visualize(Model *mod, Camera *cam)
Pose & Load(Worldfile *wf, int section, const char *keyword)
Definition: model.cc:180
void Visualize(Vis *vis, ModelRanger *rgr) const
std::vector< Sensor > sensors
Definition: stage.hh:2814
virtual void PopColor()
Definition: stage.hh:2191
float s
Definition: glutgraphics.cc:58
static bool ranger_match(Model *hit, Model *finder, const void *dummy)
bool thread_safe
Definition: stage.hh:1964
World * GetWorld() const
Definition: stage.hh:2302
meters_t z
location in 3 axes
Definition: stage.hh:251
meters_t y
Definition: stage.hh:251
Worldfile * wf
Definition: stage.hh:2015
double watts_t
Definition: stage.hh:195
bool IsRelated(const Model *testmod) const
Definition: model.cc:570
Pose origin
Definition: stage.hh:766
virtual void Load()
virtual void Print(char *prefix) const
Definition: model.cc:680
double r
Definition: stage.hh:200
static Option showArea
Definition: stage.hh:2761
static Option showBeams
Definition: stage.hh:2764
void LoadSensor(Worldfile *wf, int entity)
meters_t range
range to beam hit in meters
Definition: stage.hh:745
void Load(Worldfile *wf, int entity)
RaytraceResult Raytrace(const Ray &ray)
Definition: world.cc:775
const std::vector< Sensor > & GetSensors() const
Definition: stage.hh:2804
void pose_shift(const Pose &pose)
Definition: gl.cc:13
virtual void Print(char *prefix) const
void AddVisualizer(Visualizer *custom_visual, bool on_by_default)
Definition: model_draw.cc:322
virtual void Startup()
Definition: model.cc:707
double b
Definition: stage.hh:200
void draw_string(float x, float y, float z, const char *string)
Definition: gl.cc:65
virtual void Update()
Definition: model.cc:735
double g
Definition: stage.hh:200
void Update(ModelRanger *rgr)
virtual void Startup()
virtual void Load()
Definition: model.cc:1422
class Stg::Model::Visibility vis
ModelRanger class
Definition: stage.hh:2747
Pose pose
Definition: stage.hh:1916
static Option showTransducers
Definition: stage.hh:2765
void SetWatts(watts_t watts)
Definition: model.cc:1338
World * world
Definition: stage.hh:2017
static Option showFov
Definition: stage.hh:2763
int ReadInt(int entity, const char *name, int value)
Definition: worldfile.cc:1398
void SetGeom(const Geom &src)
Definition: model.cc:1243
virtual void Shutdown()
Definition: model.cc:723
#define FOR_EACH(I, C)
Definition: stage.hh:616
radians_t a
rotation about the z axis.
Definition: stage.hh:252
static Option showStrikes
Definition: stage.hh:2762
#define PRINT_DEBUG(m)
Definition: stage.hh:666
double ReadAngle(int entity, const char *name, double value)
Definition: worldfile.hh:116
virtual void Shutdown()
virtual void Update()
Pose LocalToGlobal(const Pose &pose) const
Definition: stage.hh:2435
void SetColor(Color col)
Definition: model.cc:1260
meters_t x
Definition: stage.hh:251
virtual ~ModelRanger()
int sgn(int a)
Definition: stage.hh:162
void ClearBlocks()
Definition: model.cc:421


stage
Author(s): Richard Vaughan , Brian Gerkey , Reed Hedges , Andrew Howard , Toby Collett , Pooya Karimian , Jeremy Asher , Alex Couture-Beil , Geoff Biggs , Rich Mattes , Abbas Sadat
autogenerated on Mon Jun 10 2019 15:06:09