map_canvas.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 
31 #include <GL/glew.h>
32 #include <GL/gl.h>
33 #include <GL/glu.h>
34 
35 #include <mapviz/map_canvas.h>
36 
37 // C++ standard libraries
38 #include <cmath>
40 
41 namespace mapviz
42 {
43 
44 
46 {
47  return a->DrawOrder() < b->DrawOrder();
48 }
49 
50 MapCanvas::MapCanvas(QWidget* parent) :
51  QGLWidget(QGLFormat(QGL::SampleBuffers), parent),
52  has_pixel_buffers_(false),
53  pixel_buffer_size_(0),
54  pixel_buffer_index_(0),
55  capture_frames_(false),
56  initialized_(false),
57  fix_orientation_(false),
58  rotate_90_(false),
59  enable_antialiasing_(true),
60  mouse_button_(Qt::NoButton),
61  mouse_pressed_(false),
62  mouse_x_(0),
63  mouse_y_(0),
64  mouse_previous_y_(0),
65  mouse_hovering_(false),
66  mouse_hover_x_(0),
67  mouse_hover_y_(0),
68  offset_x_(0),
69  offset_y_(0),
70  drag_x_(0),
71  drag_y_(0),
72  view_center_x_(0),
73  view_center_y_(0),
74  view_scale_(1),
75  view_left_(-25),
76  view_right_(25),
77  view_top_(10),
78  view_bottom_(-10),
79  scene_left_(-10),
80  scene_right_(10),
81  scene_top_(10),
82  scene_bottom_(-10)
83 {
84  ROS_INFO("View scale: %f meters/pixel", view_scale_);
85  setMouseTracking(true);
86 
88 
89  QObject::connect(&frame_rate_timer_, SIGNAL(timeout()), this, SLOT(update()));
90  setFrameRate(50.0);
91  frame_rate_timer_.start();
92  setFocusPolicy(Qt::StrongFocus);
93 }
94 
96 {
97  if(pixel_buffer_size_ != 0)
98  {
99  glDeleteBuffersARB(2, pixel_buffer_ids_);
100  }
101 }
102 
104 {
105  tf_ = tf;
106 }
107 
109 {
111  {
112  int32_t buffer_size = width() * height() * 4;
113 
114  if (pixel_buffer_size_ != buffer_size)
115  {
116  if (pixel_buffer_size_ != 0)
117  {
118  glDeleteBuffersARB(2, pixel_buffer_ids_);
119  }
120 
121  glGenBuffersARB(2, pixel_buffer_ids_);
122  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[0]);
123  glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, buffer_size, 0, GL_STREAM_READ_ARB);
124  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[1]);
125  glBufferDataARB(GL_PIXEL_PACK_BUFFER_ARB, buffer_size, 0, GL_STREAM_READ_ARB);
126  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
127 
128  pixel_buffer_size_ = buffer_size;
129  }
130  }
131 }
132 
134 {
135  GLenum err = glewInit();
136  if (GLEW_OK != err)
137  {
138  ROS_ERROR("Error: %s\n", glewGetErrorString(err));
139  }
140  else
141  {
142  // Check if pixel buffers are available for asynchronous capturing
143  std::string extensions = (const char*)glGetString(GL_EXTENSIONS);
144  has_pixel_buffers_ = extensions.find("GL_ARB_pixel_buffer_object") != std::string::npos;
145  }
146 
147  glClearColor(0.58f, 0.56f, 0.5f, 1);
149  {
150  glEnable(GL_MULTISAMPLE);
151  glEnable(GL_POINT_SMOOTH);
152  glEnable(GL_LINE_SMOOTH);
153  glEnable(GL_POLYGON_SMOOTH);
154  glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
155  glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
156  glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST);
157  }
158  else
159  {
160  glDisable(GL_MULTISAMPLE);
161  glDisable(GL_POINT_SMOOTH);
162  glDisable(GL_LINE_SMOOTH);
163  glDisable(GL_POLYGON_SMOOTH);
164  }
165  initGlBlending();
166 
167  initialized_ = true;
168 }
169 
171 {
172  glEnable(GL_BLEND);
173  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
174  glDepthFunc(GL_NEVER);
175  glDisable(GL_DEPTH_TEST);
176 }
177 
178 void MapCanvas::resizeGL(int w, int h)
179 {
180  UpdateView();
181 }
182 
183 void MapCanvas::CaptureFrame(bool force)
184 {
185  // Ensure the pixel size is actually 4
186  glPixelStorei(GL_PACK_ALIGNMENT, 4);
187 
188  if (has_pixel_buffers_ && !force)
189  {
191 
193  int32_t next_index = (pixel_buffer_index_ + 1) % 2;
194 
195  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[pixel_buffer_index_]);
196  glReadPixels(0, 0, width(), height(), GL_BGRA, GL_UNSIGNED_BYTE, 0);
197  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, pixel_buffer_ids_[next_index]);
198  GLubyte* data = reinterpret_cast<GLubyte*>(glMapBufferARB(GL_PIXEL_PACK_BUFFER_ARB, GL_READ_ONLY_ARB));
199  if(data)
200  {
202 
203  memcpy(&capture_buffer_[0], data, pixel_buffer_size_);
204 
205  glUnmapBufferARB(GL_PIXEL_PACK_BUFFER_ARB);
206  }
207  glBindBufferARB(GL_PIXEL_PACK_BUFFER_ARB, 0);
208  }
209  else
210  {
211  int32_t buffer_size = width() * height() * 4;
212  capture_buffer_.clear();
213  capture_buffer_.resize(buffer_size);
214 
215  glReadPixels(0, 0, width(), height(), GL_BGRA, GL_UNSIGNED_BYTE, &capture_buffer_[0]);
216  }
217 }
218 
219 void MapCanvas::paintEvent(QPaintEvent* event)
220 {
221  if (capture_frames_)
222  {
223  CaptureFrame();
224  }
225 
226  QPainter p(this);
227  p.setRenderHints(QPainter::Antialiasing |
228  QPainter::TextAntialiasing |
229  QPainter::SmoothPixmapTransform |
230  QPainter::HighQualityAntialiasing,
232  p.beginNativePainting();
233  // .beginNativePainting() disables blending and clears a handful of other
234  // values that we need to manually reset.
235  initGlBlending();
236  glMatrixMode(GL_MODELVIEW);
237  glPushMatrix();
238 
239  glClearColor(bg_color_.redF(), bg_color_.greenF(), bg_color_.blueF(), 1.0f);
240  UpdateView();
241  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
242 
243  TransformTarget(&p);
244 
245  // Draw test pattern
246  glLineWidth(3);
247  glBegin(GL_LINES);
248  // Red line to the right
249  glColor3f(1, 0, 0);
250  glVertex2f(0, 0);
251  glVertex2f(20, 0);
252 
253  // Green line to the top
254  glColor3f(0, 1, 0);
255  glVertex2f(0, 0);
256  glVertex2f(0, 20);
257  glEnd();
258 
259  std::list<MapvizPluginPtr>::iterator it;
260  for (it = plugins_.begin(); it != plugins_.end(); ++it)
261  {
262  // Before we let a plugin do any drawing, push all matrices and attributes.
263  // This helps to ensure that plugins can't accidentally mess something up
264  // for the next plugin.
265  pushGlMatrices();
266 
267  (*it)->DrawPlugin(view_center_x_, view_center_y_, view_scale_);
268 
269  if ((*it)->SupportsPainting())
270  {
271  p.endNativePainting();
272  (*it)->PaintPlugin(&p, view_center_x_, view_center_y_, view_scale_);
273  p.beginNativePainting();
274  initGlBlending();
275  }
276 
277  popGlMatrices();
278  }
279 
280  glMatrixMode(GL_MODELVIEW);
281  glPopMatrix();
282  p.endNativePainting();
283 }
284 
286 {
287  glMatrixMode(GL_TEXTURE);
288  glPushMatrix();
289  glMatrixMode(GL_PROJECTION);
290  glPushMatrix();
291  glMatrixMode(GL_MODELVIEW);
292  glPushMatrix();
293  glPushAttrib(GL_ALL_ATTRIB_BITS);
294 }
295 
297 {
298  glPopAttrib();
299  glMatrixMode(GL_MODELVIEW);
300  glPopMatrix();
301  glMatrixMode(GL_PROJECTION);
302  glPopMatrix();
303  glMatrixMode(GL_TEXTURE);
304  glPopMatrix();
305 }
306 
307 void MapCanvas::wheelEvent(QWheelEvent* e)
308 {
309  float numDegrees = e->delta() / -8;
310 
311  Zoom(numDegrees / 10.0);
312 }
313 
314 void MapCanvas::Zoom(float factor)
315 {
316  view_scale_ *= std::pow(1.1, factor);
317  UpdateView();
318 }
319 
320 void MapCanvas::mousePressEvent(QMouseEvent* e)
321 {
322  mouse_x_ = e->x();
323  mouse_y_ = e->y();
325  drag_x_ = 0;
326  drag_y_ = 0;
327  mouse_pressed_ = true;
328  mouse_button_ = e->button();
329 }
330 
331 void MapCanvas::keyPressEvent(QKeyEvent* event)
332 {
333  std::list<MapvizPluginPtr>::iterator it;
334  for (it = plugins_.begin(); it != plugins_.end(); ++it)
335  {
336  (*it)->event(event);
337  }
338 }
339 
340 QPointF MapCanvas::MapGlCoordToFixedFrame(const QPointF& point)
341 {
342  bool invertible = true;
343  return qtransform_.inverted(&invertible).map(point);
344 }
345 
346 QPointF MapCanvas::FixedFrameToMapGlCoord(const QPointF& point)
347 {
348  return qtransform_.map(point);
349 }
350 
351 void MapCanvas::mouseReleaseEvent(QMouseEvent* e)
352 {
353  mouse_button_ = Qt::NoButton;
354  mouse_pressed_ = false;
355  offset_x_ += drag_x_;
356  offset_y_ += drag_y_;
357  drag_x_ = 0;
358  drag_y_ = 0;
359 }
360 
361 void MapCanvas::mouseMoveEvent(QMouseEvent* e)
362 {
364  {
365  int diff;
366  switch (mouse_button_)
367  {
368  case Qt::LeftButton:
369  case Qt::MiddleButton:
370  if (((mouse_x_ - e->x()) != 0 || (mouse_y_ - e->y()) != 0))
371  {
372  drag_x_ = -((mouse_x_ - e->x()) * view_scale_);
373  drag_y_ = ((mouse_y_ - e->y()) * view_scale_);
374  }
375  break;
376  case Qt::RightButton:
377  diff = e->y() - mouse_previous_y_;
378  if (diff != 0)
379  {
380  Zoom(((float)diff) / 10.0f);
381  }
382  mouse_previous_y_ = e->y();
383  break;
384  default:
385  // Unexpected mouse button
386  break;
387  }
388  }
389 
390  double center_x = -offset_x_ - drag_x_;
391  double center_y = -offset_y_ - drag_y_;
392  double x = center_x + (e->x() - width() / 2.0) * view_scale_;
393  double y = center_y + (height() / 2.0 - e->y()) * view_scale_;
394 
395  tf::Point point(x, y, 0);
396  point = transform_ * point;
397 
398  mouse_hovering_ = true;
399  mouse_hover_x_ = e->x();
400  mouse_hover_y_ = e->y();
401 
402  Q_EMIT Hover(point.x(), point.y(), view_scale_);
403 }
404 
405 void MapCanvas::leaveEvent(QEvent* e)
406 {
407  mouse_hovering_ = false;
408  Q_EMIT Hover(0, 0, 0);
409 }
410 
411 void MapCanvas::SetFixedFrame(const std::string& frame)
412 {
413  fixed_frame_ = frame;
414  std::list<MapvizPluginPtr>::iterator it;
415  for (it = plugins_.begin(); it != plugins_.end(); ++it)
416  {
417  (*it)->SetTargetFrame(frame);
418  }
419 }
420 
421 void MapCanvas::SetTargetFrame(const std::string& frame)
422 {
423  offset_x_ = 0;
424  offset_y_ = 0;
425  drag_x_ = 0;
426  drag_y_ = 0;
427 
428  target_frame_ = frame;
429 }
430 
432 {
433  fix_orientation_ = on;
434 }
435 
437 {
438  rotate_90_ = on;
439 }
440 
442 {
444  QGLFormat format;
445  format.setSwapInterval(1);
446  format.setSampleBuffers(enable_antialiasing_);
447  // After setting the format, initializeGL will automatically be called again, then paintGL.
448  this->setFormat(format);
449 }
450 
452 {
453  std::list<MapvizPluginPtr>::iterator it;
454  for (it = plugins_.begin(); it != plugins_.end(); ++it)
455  {
456  (*it)->SetUseLatestTransforms(on);
457  }
458 }
459 
460 void MapCanvas::AddPlugin(MapvizPluginPtr plugin, int order)
461 {
462  plugins_.push_back(plugin);
463 }
464 
466 {
467 
468  plugin->Shutdown();
469  plugins_.remove(plugin);
470 
471 }
472 
473 void MapCanvas::TransformTarget(QPainter* painter)
474 {
475  glTranslatef(offset_x_ + drag_x_, offset_y_ + drag_y_, 0);
476  // In order for plugins drawing with a QPainter to be able to use the same coordinates
477  // as plugins using drawing using native GL commands, we have to replicate the
478  // GL transforms using a QTransform. Note that a QPainter's coordinate system is
479  // flipped on the Y axis relative to OpenGL's.
481 
484 
485  if (!tf_ || fixed_frame_.empty() || target_frame_.empty() || target_frame_ == "<none>")
486  {
487  qtransform_ = qtransform_.scale(1, -1);
488  painter->setWorldTransform(qtransform_, false);
489 
490  return;
491  }
492 
493  bool success = false;
494 
495  try
496  {
497  tf_->lookupTransform(fixed_frame_, target_frame_, ros::Time(0), transform_);
498 
499  // If the viewer orientation is fixed don't rotate the center point.
500  if (fix_orientation_)
501  {
503  }
504 
505  if (rotate_90_)
506  {
509  }
510 
511  double roll, pitch, yaw;
512  transform_.getBasis().getRPY(roll, pitch, yaw);
513 
514  glRotatef(-yaw * 57.2957795, 0, 0, 1);
515  qtransform_ = qtransform_.rotateRadians(yaw);
516 
517  glTranslatef(-transform_.getOrigin().getX(), -transform_.getOrigin().getY(), 0);
518  qtransform_ = qtransform_.translate(-transform_.getOrigin().getX(), transform_.getOrigin().getY());
519 
521 
522  tf::Point center = transform_ * point;
523 
524  view_center_x_ = center.getX();
525  view_center_y_ = center.getY();
526 
527  qtransform_ = qtransform_.scale(1, -1);
528  painter->setWorldTransform(qtransform_, false);
529 
530  if (mouse_hovering_)
531  {
532  double center_x = -offset_x_ - drag_x_;
533  double center_y = -offset_y_ - drag_y_;
534  double x = center_x + (mouse_hover_x_ - width() / 2.0) * view_scale_;
535  double y = center_y + (height() / 2.0 - mouse_hover_y_) * view_scale_;
536 
537  tf::Point hover(x, y, 0);
538  hover = transform_ * hover;
539 
540  Q_EMIT Hover(hover.x(), hover.y(), view_scale_);
541  }
542 
543  success = true;
544  }
545  catch (const tf::LookupException& e)
546  {
547  ROS_ERROR_THROTTLE(2.0, "%s", e.what());
548  }
549  catch (const tf::ConnectivityException& e)
550  {
551  ROS_ERROR_THROTTLE(2.0, "%s", e.what());
552  }
553  catch (const tf::ExtrapolationException& e)
554  {
555  ROS_ERROR_THROTTLE(2.0, "%s", e.what());
556  }
557  catch (...)
558  {
559  ROS_ERROR_THROTTLE(2.0, "Error looking up transform");
560  }
561 
562  if (!success)
563  {
564  qtransform_ = qtransform_.scale(1, -1);
565  painter->setWorldTransform(qtransform_, false);
566  }
567 }
568 
570 {
571  if (initialized_)
572  {
573  Recenter();
574 
575  glViewport(0, 0, width(), height());
576  glMatrixMode(GL_PROJECTION);
577  glLoadIdentity();
578  glOrtho(view_left_, view_right_, view_top_, view_bottom_, -0.5f, 0.5f);
579 
580  qtransform_ = QTransform::fromTranslate(width() / 2.0, height() / 2.0).
581  scale(1.0 / view_scale_, 1.0 / view_scale_);
582  }
583 }
584 
586 {
588  SetViewScale(1.0);
589 }
590 
592 {
594 }
595 
597 {
598  // Recalculate the bounds of the view
599  view_left_ = -(width() * view_scale_ * 0.5);
600  view_top_ = -(height() * view_scale_ * 0.5);
601  view_right_ = (width() * view_scale_ * 0.5);
602  view_bottom_ = (height() * view_scale_ * 0.5);
603 }
604 
605 void MapCanvas::setFrameRate(const double fps)
606 {
607  if (fps <= 0.0) {
608  ROS_ERROR("Invalid frame rate: %f", fps);
609  return;
610  }
611 
612  frame_rate_timer_.setInterval(1000.0/fps);
613 }
614 
615 double MapCanvas::frameRate() const
616 {
617  return 1000.0 / frame_rate_timer_.interval();
618 }
619 } // namespace mapviz
static const long double _half_pi
void mouseMoveEvent(QMouseEvent *e)
Definition: map_canvas.cpp:361
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
QPointF MapGlCoordToFixedFrame(const QPointF &point)
Definition: map_canvas.cpp:340
void RemovePlugin(MapvizPluginPtr plugin)
Definition: map_canvas.cpp:465
void setFrameRate(const double fps)
Definition: map_canvas.cpp:605
Quaternion getRotation() const
void TransformTarget(QPainter *painter)
Definition: map_canvas.cpp:473
f
void CaptureFrame(bool force=false)
Definition: map_canvas.cpp:183
void resizeGL(int w, int h)
Definition: map_canvas.cpp:178
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
std::vector< uint8_t > capture_buffer_
Definition: map_canvas.h:252
boost::shared_ptr< tf::TransformListener > tf_
Definition: map_canvas.h:247
void keyPressEvent(QKeyEvent *e)
Definition: map_canvas.cpp:331
void leaveEvent(QEvent *e)
Definition: map_canvas.cpp:405
void InitializeTf(boost::shared_ptr< tf::TransformListener > tf)
Definition: map_canvas.cpp:103
Qt::MouseButton mouse_button_
Definition: map_canvas.h:206
data
QTimer frame_rate_timer_
Definition: map_canvas.h:202
tf::StampedTransform transform_
Definition: map_canvas.h:248
void ToggleEnableAntialiasing(bool on)
Definition: map_canvas.cpp:441
void SetFixedFrame(const std::string &frame)
Definition: map_canvas.cpp:411
tf::Vector3 Point
std::string fixed_frame_
Definition: map_canvas.h:244
int32_t pixel_buffer_index_
Definition: map_canvas.h:194
bool compare_plugins(MapvizPluginPtr a, MapvizPluginPtr b)
Definition: map_canvas.cpp:45
void setIdentity()
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool canvas_able_to_move_
Definition: map_canvas.h:190
int32_t pixel_buffer_size_
Definition: map_canvas.h:192
bool enable_antialiasing_
Definition: map_canvas.h:200
void wheelEvent(QWheelEvent *e)
Definition: map_canvas.cpp:307
void mouseReleaseEvent(QMouseEvent *e)
Definition: map_canvas.cpp:351
GLuint pixel_buffer_ids_[2]
Definition: map_canvas.h:193
QTransform qtransform_
Definition: map_canvas.h:249
static Quaternion createQuaternionFromYaw(double yaw)
QPointF FixedFrameToMapGlCoord(const QPointF &point)
Definition: map_canvas.cpp:346
#define ROS_INFO(...)
double frameRate() const
Definition: map_canvas.cpp:615
#define ROS_ERROR_THROTTLE(period,...)
void InitializePixelBuffers()
Definition: map_canvas.cpp:108
void ToggleRotate90(bool on)
Definition: map_canvas.cpp:436
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void Hover(double x, double y, double scale)
std::string target_frame_
Definition: map_canvas.h:245
void ToggleUseLatestTransforms(bool on)
Definition: map_canvas.cpp:451
void SetTargetFrame(const std::string &frame)
Definition: map_canvas.cpp:421
std::list< MapvizPluginPtr > plugins_
Definition: map_canvas.h:250
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
static const Transform & getIdentity()
void ToggleFixOrientation(bool on)
Definition: map_canvas.cpp:431
void paintEvent(QPaintEvent *event)
Definition: map_canvas.cpp:219
void Zoom(float factor)
Definition: map_canvas.cpp:314
void SetViewScale(float scale)
Definition: map_canvas.h:95
void mousePressEvent(QMouseEvent *e)
Definition: map_canvas.cpp:320
void AddPlugin(MapvizPluginPtr plugin, int order)
Definition: map_canvas.cpp:460
#define ROS_ERROR(...)
MapCanvas(QWidget *parent=0)
Definition: map_canvas.cpp:50


mapviz
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:30