rock_widget_collection  0.1
Public Slots | Public Member Functions | Protected Member Functions | List of all members
SonarDisplay Class Reference

#include <SonarDisplay.h>

Inherits QVTKWidget.

Public Slots

void addSonarBeam (float bearing, int number_of_bins, const char *pbuffer)
 
void setUpSonar (int number_of_beams, int number_of_bins, float horizontal_resolution, float distance_resolution, float vertical_resolution)
 
void setParameterToAuto (bool value)
 
void deleteBeams (int from, int to)
 
int getNumberOfBins ()
 

Public Member Functions

 SonarDisplay (QWidget *parent=NULL)
 
virtual ~SonarDisplay ()
 
vtkSmartPointer< vtkStructuredGrid > generateStructuredGrid (int number_of_beams, int number_of_bins, float horizontal_resolution, float distance_resolution, float vertical_resolution=0)
 
void setUpStructuredGrid (vtkSmartPointer< vtkStructuredGrid > sgrid, int number_of_beams, int number_of_bins, float horizontal_resolution, float distance_resolution, float vertical_resolution, float start_angle=0)
 
int getActorID (const vtkActor *actor)
 
vtkStructuredGridGeometryFilter * getFilter (int id)
 
vtkUnsignedCharArray * getData (int id)
 
int getNumberOfBeams ()
 
void setPlotData (int id)
 
void reset ()
 
bool isPlotVisible ()
 
void setPlotVisible (bool val)
 

Protected Member Functions

int normalizeDelata (int delta)
 

Detailed Description

Definition at line 51 of file SonarDisplay.h.

Constructor & Destructor Documentation

SonarDisplay::SonarDisplay ( QWidget *  parent = NULL)

Definition at line 101 of file SonarDisplay.cc.

101  :
102  QVTKWidget(parent),
103  number_of_beams(120), //121
104  number_of_bins(500),
105  start_bearing(0),
106  end_bearing(2*M_PI),
107  horizontal_resolution(3.0/180.0*M_PI),
108  vertical_resolution(30.0/180.0*M_PI),
109  distance_resolution(0.1),
110  paramerter_auto(false),
111  last_index(0),
112  key_event(this),
113  sonar_wireframe(vtkSmartPointer<vtkStructuredGrid>::New()),
114  render_window(vtkSmartPointer<vtkRenderWindow>::New()),
115  renderer(vtkSmartPointer<vtkRenderer>::New()),
116  plot_actor(vtkSmartPointer<vtkXYPlotActor>::New())
117 {
118  this->resize(256,256);
119  render_window->AddRenderer(renderer);
120  this->SetRenderWindow(render_window);
121  render_window->GetInteractor()->AddObserver(vtkCommand::KeyReleaseEvent,&key_event,0.0);
122 
123  setUpSonar(number_of_beams,number_of_bins,horizontal_resolution,distance_resolution,vertical_resolution);
124 
125 
126  //register events
127 }
SonarDisplay::setUpSonar
void setUpSonar(int number_of_beams, int number_of_bins, float horizontal_resolution, float distance_resolution, float vertical_resolution)
Definition: SonarDisplay.cc:197
virtual SonarDisplay::~SonarDisplay ( )
inlinevirtual

Definition at line 58 of file SonarDisplay.h.

58 {};

Member Function Documentation

void SonarDisplay::addSonarBeam ( float  bearing,
int  number_of_bins,
const char *  pbuffer 
)
slot

Definition at line 364 of file SonarDisplay.cc.

365 {
366  //check parameter
367  if(start_bearing > bearing || end_bearing < bearing)
368  {
369  std::cout << "SonarDisplay: Bearing is out of range!" << std::endl;
370  return;
371  }
372 
373  if(number_of_bins > this->number_of_bins)
374  {
375  std::cout << "SonarDisplay: Bin is out of range!" << std::endl;
376  return;
377  }
378 
379 
380  //copy sonar data
381  //get index of for the sonar beam
382  int index = bearing/horizontal_resolution+0.5;
383  if((unsigned int)index >= sonar_data.size())
384  index = 0;
385 
386  int delta =normalizeDelata(index-last_index);
387  bool scan_right;
388  if(delta < 0)
389  scan_right = false;
390  else
391  scan_right = true;
392 
393  int start_index;
394  int end_index;
395 
396  if(index == last_index)
397  {
398  start_index = index;
399  end_index = index;
400  }
401  else
402  {
403  if(scan_right)
404  {
405  start_index = last_index+1;
406  end_index = index;
407  }
408  else
409  {
410  start_index = index;
411  end_index = last_index-1;
412  if(end_index < 0)
413  end_index = number_of_beams-1;
414  }
415  }
416 
417  //find oldest
418  std::vector<vtkSmartPointer<vtkUnsignedCharArray> >::iterator iter2 = sonar_data.begin();
419  std::vector<vtkSmartPointer<vtkUnsignedCharArray> >::iterator oldest = sonar_data.begin();
420  for(;iter2 != sonar_data.end();++iter2)
421  {
422  if((*oldest)->GetMTime() > (*iter2)->GetMTime())
423  oldest = iter2;
424  }
425 
426  do
427  {
428  if(start_index >= number_of_beams)
429  start_index = 0;
430  void* p = sonar_data[start_index]->WriteVoidPointer(0,number_of_bins);
431  memcpy(p,pbuffer,number_of_bins);
432  sonar_data[start_index]->Modified();
433  }while(start_index != end_index && ++start_index);
434 
435  if(last_index%3 && last_index != index && abs((*oldest)->GetMTime() - sonar_data[end_index]->GetMTime()) > 10e4)
436  {
437  void* p = (*oldest)->WriteVoidPointer(0,number_of_bins);
438  memset(p,0,number_of_bins);
439  (*oldest)->Modified();
440  }
441 
442  last_index = index;
443  render_window->Render();
444 }
SonarDisplay::normalizeDelata
int normalizeDelata(int delta)
Definition: SonarDisplay.cc:446
void SonarDisplay::deleteBeams ( int  from,
int  to 
)
slot

Definition at line 354 of file SonarDisplay.cc.

355 {
356  for(int i = from;i <= to;++i)
357  {
358  void* p = sonar_data[i]->WriteVoidPointer(0,number_of_bins);
359  memset(p,0,number_of_bins);
360  sonar_data[i]->Modified();
361  }
362 }
vtkSmartPointer<vtkStructuredGrid> SonarDisplay::generateStructuredGrid ( int  number_of_beams,
int  number_of_bins,
float  horizontal_resolution,
float  distance_resolution,
float  vertical_resolution = 0 
)
int SonarDisplay::getActorID ( const vtkActor *  actor)

Definition at line 129 of file SonarDisplay.cc.

130 {
131  std::vector<vtkSmartPointer<vtkActor> >::iterator iter = sonar_actor.begin();
132  for(int i=0;iter != sonar_actor.end();++iter,++i)
133  if((*iter).GetPointer() == actor)
134  return i;
135 
136  return -1;
137 }
vtkUnsignedCharArray * SonarDisplay::getData ( int  id)

Definition at line 143 of file SonarDisplay.cc.

144 {
145  return sonar_data[id];
146 }
vtkStructuredGridGeometryFilter * SonarDisplay::getFilter ( int  id)

Definition at line 139 of file SonarDisplay.cc.

140 {
141  return sonar_filter[id];
142 }
int SonarDisplay::getNumberOfBeams ( )
inline

Definition at line 81 of file SonarDisplay.h.

81 {return number_of_beams;};
int SonarDisplay::getNumberOfBins ( )
inlineslot

Definition at line 80 of file SonarDisplay.h.

80 {return number_of_bins;};
bool SonarDisplay::isPlotVisible ( )

Definition at line 159 of file SonarDisplay.cc.

160 {
161  return plot_actor->GetVisibility();
162 }
int SonarDisplay::normalizeDelata ( int  delta)
protected

Definition at line 446 of file SonarDisplay.cc.

447 {
448  if(delta > 0.5*number_of_beams)
449  return delta - number_of_beams;
450  else if(delta < -0.5*number_of_beams)
451  return delta + number_of_beams;
452  return delta;
453 }
void SonarDisplay::reset ( )

Definition at line 170 of file SonarDisplay.cc.

171 {
172  std::vector<vtkSmartPointer<vtkStructuredGridGeometryFilter> >::iterator iter = sonar_filter.begin();
173  while(iter != sonar_filter.end())
174  {
175  (*iter)->SetExtent(0,1,0,number_of_bins,1,1);
176  (*iter)->Modified();
177  ++iter;
178  }
179 
180  renderer->SetBackground(0.1,0.1,0.1);
181  renderer->GetActiveCamera()->SetPosition(0,0,1);
182  //renderer->GetActiveCamera()->Elevation(0.0);
183  //renderer->GetActiveCamera()->Azimuth(0.0);
184  //renderer->GetActiveCamera()->Dolly(0.0);
185  //renderer->GetActiveCamera()->SetViewUp(0,1,0);
186  renderer->GetActiveCamera()->Yaw(0.0);
187  renderer->GetActiveCamera()->Roll(0.0);
188  renderer->GetActiveCamera()->Pitch(0.0);
189  renderer->GetActiveCamera()->SetFocalPoint(0,0,0);
190  renderer->ResetCamera();
191  renderer->GetActiveCamera()->ParallelProjectionOn();
192  renderer->GetActiveCamera()->Zoom(1.40);
193  render_window->Render();
194 }
void SonarDisplay::setParameterToAuto ( bool  value)
slot

Definition at line 348 of file SonarDisplay.cc.

349 {
350  paramerter_auto = value;
351 }
void SonarDisplay::setPlotData ( int  id)

Definition at line 148 of file SonarDisplay.cc.

149 {
150  if(!isPlotVisible())
151  return;
152  vtkSmartPointer<vtkFieldData> field = vtkSmartPointer<vtkFieldData>::New();
153  vtkSmartPointer<vtkDataObject> data = vtkSmartPointer<vtkDataObject>::New();
154  field->AddArray(sonar_data[id]);
155  data->SetFieldData(field);
156  plot_actor->RemoveAllInputs();
157  plot_actor->AddDataObjectInput(data);
158 }
SonarDisplay::isPlotVisible
bool isPlotVisible()
Definition: SonarDisplay.cc:159
void SonarDisplay::setPlotVisible ( bool  val)

Definition at line 164 of file SonarDisplay.cc.

165 {
166  plot_actor->SetVisibility(val);
167 }
void SonarDisplay::setUpSonar ( int  number_of_beams,
int  number_of_bins,
float  horizontal_resolution,
float  distance_resolution,
float  vertical_resolution 
)
slot

Definition at line 197 of file SonarDisplay.cc.

200 {
201  //check some paramerters
202  if(number_of_beams < 10 || number_of_bins < 10 || horizontal_resolution < 0||
203  horizontal_resolution > M_PI*0.3 || distance_resolution < 0.01 || vertical_resolution > M_PI*0.45)
204  throw std::runtime_error("invalid configuration");
205 
206  renderer->RemoveAllViewProps();
207  sonar_wireframe->ReleaseData();
208  sonar_wireframe->RemoveAllObservers();
209  sonar_actor.clear();
210  sonar_mapper.clear();
211  sonar_filter.clear();
212  sonar_grid.clear();
213  sonar_data.clear();
214 
215  sonar_wireframe = vtkSmartPointer<vtkStructuredGrid>::New();
216 
217  this->number_of_beams = number_of_beams;
218  this->number_of_bins = number_of_bins;
219  // this->start_bearing = start_bearing;
220  // this->end_bearing = end_bearing;
221  this->horizontal_resolution = horizontal_resolution;
222  this->vertical_resolution = vertical_resolution;
223  this->distance_resolution = distance_resolution;
224 
225  int missing = number_of_beams - sonar_data.size();
226  for(;missing > 0 ;--missing)
227  {
228  sonar_data.push_back(vtkSmartPointer<vtkUnsignedCharArray>::New());
229  sonar_grid.push_back(vtkSmartPointer<vtkStructuredGrid>::New());
230  }
231 
232  std::vector<vtkSmartPointer<vtkStructuredGrid> >::iterator iter = sonar_grid.begin();
233  std::vector<vtkSmartPointer<vtkUnsignedCharArray> >::iterator iter2 = sonar_data.begin();
234  for(float val=0;iter != sonar_grid.end();val+=horizontal_resolution, ++iter,++iter2)
235  {
236  setUpStructuredGrid(*iter,2,number_of_bins+1,horizontal_resolution,distance_resolution,vertical_resolution,val);
237  void *p = (*iter2)->WriteVoidPointer(0,number_of_bins);
238  memset(p,0,number_of_bins);
239  (*iter)->GetCellData()->SetScalars(*iter2);
240  (*iter2)->Modified();
241  }
242 
243  // Create wireframe
244  // every 2 meter and every 15 degrees
245  setUpStructuredGrid(sonar_wireframe,25,number_of_bins*distance_resolution*0.5,15.0/180.0*M_PI,2,vertical_resolution);
246 
247 
248  //set up pipeline (mapper + actors)
249  vtkLookupTable *lut = vtkLookupTable::New();
250  lut->SetNumberOfColors(255);
251  lut->SetHueRange(0.55,1);
252  lut->SetValueRange(1,1.0);
253  lut->SetSaturationRange(1,1.0);
254  lut->SetTableRange(0,255);
255  lut->Build();
256  iter = sonar_grid.begin();
257  for(;iter != sonar_grid.end(); ++iter)
258  {
259  vtkSmartPointer<vtkStructuredGridGeometryFilter> filter = vtkSmartPointer<vtkStructuredGridGeometryFilter>::New();
260  filter->SetInputConnection((*iter)->GetProducerPort());
261  vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
262  mapper->SetInputConnection(filter->GetOutputPort());
263  mapper->SetLookupTable(lut);
264  mapper->UseLookupTableScalarRangeOn();
265  mapper->SetColorModeToMapScalars();
266  // set_mapper->SetScalarRange(0,255);
267 
268  vtkSmartPointer<vtkActor> sonar_grid_actor = vtkSmartPointer<vtkActor>::New();
269  sonar_grid_actor->SetMapper(mapper);
270  // sonar_grid_actor->GetProperty()->SetOpacity(0.80);
271 
272  sonar_filter.push_back(filter);
273  sonar_mapper.push_back(mapper);
274  sonar_actor.push_back(sonar_grid_actor);
275 
276  renderer->AddActor(sonar_grid_actor);
277  }
278  lut->Delete();
279 
280  //wireframe
281  vtkDataSetMapper *set_mapper = vtkDataSetMapper::New();
282  set_mapper->SetInputConnection(sonar_wireframe->GetProducerPort());
283  set_mapper->ScalarVisibilityOff();
284 
285  vtkSmartPointer<vtkActor> wireframe_actor = vtkSmartPointer<vtkActor>::New();
286  wireframe_actor->SetMapper(set_mapper);
287  wireframe_actor->GetProperty()->SetColor(0.5,0.5,0.5);
288  wireframe_actor->GetProperty()->SetRepresentationToWireframe();
289  wireframe_actor->GetProperty()->SetLineWidth(1.2);
290  wireframe_actor->PickableOff();
291  wireframe_actor->SetPosition(0,0,0.1);
292  set_mapper->Delete();
293 
294  renderer->AddActor(wireframe_actor);
295 
296  //plotting
297  plot_actor->ExchangeAxesOff();
298  plot_actor->SetXValuesToValue();
299  plot_actor->SetWidth(0.9);
300  plot_actor->SetXTitle("");
301  plot_actor->SetYTitle("");
302  plot_actor->SetHeight(0.50);
303  plot_actor->SetPosition(0.04,0);
304  plot_actor->PlotPointsOn();
305  plot_actor->PlotLinesOn();
306  plot_actor->SetVisibility(false);
307  renderer->AddActor(plot_actor);
308 
309  reset();
310 }
SonarDisplay::setUpStructuredGrid
void setUpStructuredGrid(vtkSmartPointer< vtkStructuredGrid > sgrid, int number_of_beams, int number_of_bins, float horizontal_resolution, float distance_resolution, float vertical_resolution, float start_angle=0)
Definition: SonarDisplay.cc:312
void SonarDisplay::setUpStructuredGrid ( vtkSmartPointer< vtkStructuredGrid >  sgrid,
int  number_of_beams,
int  number_of_bins,
float  horizontal_resolution,
float  distance_resolution,
float  vertical_resolution,
float  start_angle = 0 
)

Definition at line 312 of file SonarDisplay.cc.

315 {
316  double point[3];
317  double radius;
318  double theta;
319 
320  // Create the structured grid.
321  sgrid->SetDimensions(number_of_beams,number_of_bins,2);
322 
323  vtkPoints *points = vtkPoints::New();
324  points->Allocate(number_of_bins*number_of_beams*2);
325 
326  int k =-1;
327  if (vertical_resolution == 0)
328  k = 0;
329  for(;k<=1;k+=2)
330  {
331  for (int j=1; j<=number_of_bins; j++)
332  {
333  radius = j* distance_resolution;
334  point[2] = k*radius*sin(0.5*vertical_resolution);
335  for (int i=1; i<= number_of_beams; i++)
336  {
337  theta = horizontal_resolution*i+start_angle;
338  point[0] = radius * cos(theta);
339  point[1] = radius * sin(theta);
340  points->InsertNextPoint(point);
341  }
342  }
343  }
344  sgrid->SetPoints(points);
345  points->Delete();
346 }
The documentation for this class was generated from the following files:
Generated by   doxygen 1.8.11