point_drawing_plugin.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 
31 
32 #include <vector>
33 #include <list>
34 
35 #include <QDialog>
36 #include <QGLWidget>
37 #include <QPalette>
38 #include <QPainter>
39 
40 #include <opencv2/core/core.hpp>
41 
44 
45 namespace mapviz_plugins
46 {
48  : arrow_size_(25),
49  draw_style_(LINES),
50  position_tolerance_(0.0),
51  buffer_size_(0),
52  covariance_checked_(false),
53  show_all_covariances_checked_(false),
54  new_lap_(true),
55  lap_checked_(false),
56  buffer_holder_(false),
57  scale_(1.0),
58  static_arrow_sizes_(false),
59  got_begin_(false)
60  {
61  QObject::connect(this,
62  SIGNAL(TargetFrameChanged(const std::string&)),
63  this,
64  SLOT(ResetTransformedPoints()));
65  }
66 
68  {
69  ROS_INFO("PointDrawingPlugin::ClearHistory()");
70  points_.clear();
71  }
72 
74  {
75  if (icon_)
76  {
77  QPixmap icon(16, 16);
78  icon.fill(Qt::transparent);
79 
80  QPainter painter(&icon);
81  painter.setRenderHint(QPainter::Antialiasing, true);
82 
83  QPen pen(color_);
84 
85  if (draw_style_ == POINTS)
86  {
87  pen.setWidth(7);
88  pen.setCapStyle(Qt::RoundCap);
89  painter.setPen(pen);
90  painter.drawPoint(8, 8);
91  }
92  else if (draw_style_ == LINES)
93  {
94  pen.setWidth(3);
95  pen.setCapStyle(Qt::FlatCap);
96  painter.setPen(pen);
97  painter.drawLine(1, 14, 14, 1);
98  }
99  else if (draw_style_ == ARROWS)
100  {
101  pen.setWidth(2);
102  pen.setCapStyle(Qt::SquareCap);
103  painter.setPen(pen);
104  painter.drawLine(2, 13, 13, 2);
105  painter.drawLine(13, 2, 13, 8);
106  painter.drawLine(13, 2, 7, 2);
107  }
108 
109  icon_->SetPixmap(icon);
110  }
111  }
112 
114  {
115  arrow_size_ = arrowSize;
117  }
118 
120  {
121  if (style == "lines")
122  {
123  draw_style_ = LINES;
124  }
125  else if (style == "points")
126  {
128  }
129  else if (style == "arrows")
130  {
132  }
134  DrawIcon();
135  }
136 
138  {
139  draw_style_ = style;
140  DrawIcon();
141  }
142 
144  {
145  static_arrow_sizes_ = isChecked;
147  }
148 
150  {
151  position_tolerance_ = value;
152  }
153 
155  {
156  lap_checked_ = checked;
157  }
158 
160  {
161  covariance_checked_ = checked;
162  }
163 
165  {
167  }
168 
170  {
171  for (std::deque<StampedPoint>& lap: laps_)
172  {
173  for (StampedPoint& point: lap)
174  {
175  point.transformed = false;
176  }
177  }
178  for (StampedPoint& point: points_)
179  {
180  point.transformed = false;
181  }
182  Transform();
183  }
184 
186  {
187  cur_point_ = stamped_point;
188 
189  if (points_.empty() ||
190  (stamped_point.point.distance(points_.back().point)) >=
192  {
193  points_.push_back(stamped_point);
194  }
195 
196  if (buffer_size_ > 0)
197  {
198  while (static_cast<int>(points_.size()) >= buffer_size_)
199  {
200  points_.pop_front();
201  }
202  }
203  }
204 
206  {
207  points_.clear();
208  }
209 
211  {
212  if (!lap_checked_)
213  {
214  return buffer_size_;
215  }
216  else
217  {
218  return buffer_holder_;
219  }
220  }
221 
223  {
224  return position_tolerance_;
225  }
226 
227  const std::deque<PointDrawingPlugin::StampedPoint> &PointDrawingPlugin::points() const
228  {
229  return points_;
230  }
231 
233  {
234  buffer_size_ = value;
235 
236  if (buffer_size_ > 0)
237  {
238  while (static_cast<int>(points_.size()) >= buffer_size_)
239  {
240  points_.pop_front();
241  }
242  }
243  }
244 
246  {
247  if( scale_ != scale && draw_style_ == ARROWS && static_arrow_sizes_)
248  {
250  }
251  scale_ = scale;
252  bool transformed = true;
253  if (lap_checked_)
254  {
255  CollectLaps();
256 
257  if (draw_style_ == ARROWS)
258  {
259  transformed &= DrawLapsArrows();
260  }
261  else
262  {
263  transformed &= DrawLaps();
264  }
265  }
266  else if (buffer_size_ == INT_MAX)
267  {
269  laps_.clear();
270  got_begin_ = false;
271  }
272  if (draw_style_ == ARROWS)
273  {
274  transformed &= DrawArrows();
275  }
276  else
277  {
278  transformed &= DrawLines();
279  }
280 
281  return transformed;
282  }
283 
285  {
286  if (!got_begin_)
287  {
289  points_.clear();
291  buffer_size_ = INT_MAX;
292  got_begin_ = true;
293  }
295  if (((std::fabs(check.x()) <= 3) && (std::fabs(check.y()) <= 3)) &&
296  !new_lap_)
297  {
298  new_lap_ = true;
299  if (points_.size() > 0)
300  {
301  laps_.push_back(points_);
302  laps_[0].pop_back();
303  points_.clear();
304  points_.push_back(cur_point_);
305  }
306  }
307 
308  if (((std::fabs(check.x()) > 25) && (std::fabs(check.y()) > 25)) &&
309  new_lap_)
310  {
311  new_lap_ = false;
312  }
313  }
314 
316  {
317  bool success = cur_point_.transformed;
318  glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0);
319  if (draw_style_ == LINES && points_.size()>0)
320  {
321  glLineWidth(3);
322  glBegin(GL_LINE_STRIP);
323  }
324  else
325  {
326  glPointSize(6);
327  glBegin(GL_POINTS);
328  }
329 
330  for (const auto& pt : points_)
331  {
332  success &= pt.transformed;
333  if (pt.transformed)
334  {
335  glVertex2d(pt.transformed_point.getX(), pt.transformed_point.getY());
336  }
337  }
338 
340  {
341  glVertex2d(cur_point_.transformed_point.getX(),
343  }
344 
345  glEnd();
346 
347  return success;
348  }
349 
351  {
352  if (it.transformed)
353  {
354  glVertex2d(it.transformed_point.getX(),
355  it.transformed_point.getY());
356 
357  glVertex2d(it.transformed_arrow_point.getX(),
358  it.transformed_arrow_point.getY());
359 
360  glVertex2d(it.transformed_arrow_point.getX(),
361  it.transformed_arrow_point.getY());
362  glVertex2d(it.transformed_arrow_left.getX(),
363  it.transformed_arrow_left.getY());
364 
365  glVertex2d(it.transformed_arrow_point.getX(),
366  it.transformed_arrow_point.getY());
367  glVertex2d(it.transformed_arrow_right.getX(),
368  it.transformed_arrow_right.getY());
369  return true;
370  }
371  return false;
372  }
373 
375  {
376  bool success = true;
377  glLineWidth(4);
378  glBegin(GL_LINES);
379  glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5);
380  for (const auto &pt : points_)
381  {
382  success &= DrawArrow(pt);
383  }
384 
385  success &= DrawArrow(cur_point_);
386 
387  glEnd();
388 
389  return success;
390  }
391 
392  void PointDrawingPlugin::SetColor(const QColor& color)
393  {
394  if (color != color_)
395  {
396  color_ = color;
397  DrawIcon();
398  }
399  }
400 
402  {
403  if ( point.transformed )
404  {
405  return true;
406  }
407 
409  if( GetTransform(point.source_frame, point.stamp, transform))
410  {
411  point.transformed_point = transform * point.point;
412 
413  if (draw_style_ == ARROWS)
414  {
415  tf::Transform orientation(tf::Transform(transform.GetOrientation()) *
416  point.orientation);
417 
418  double size = static_cast<double>(arrow_size_);
420  {
421  size *= scale_;
422  }
423  else
424  {
425  size /= 10.0;
426  }
427  double arrow_width = size / 5.0;
428  double head_length = size * 0.75;
429 
430  // If quaternion malformed, just draw point instead
431  const tf::Quaternion q(point.orientation);
432  if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
433  {
434  orientation = tf::Transform(tf::Transform(transform.GetOrientation()));
435  arrow_width = 0.0;
436  head_length = 0.0;
437  size = 0;
438  }
439 
441  point.transformed_point + orientation * tf::Point(size, 0.0, 0.0);
442  point.transformed_arrow_left =
443  point.transformed_point + orientation * tf::Point(head_length, -arrow_width, 0.0);
445  point.transformed_point + orientation * tf::Point(head_length, arrow_width, 0.0);
446  }
447 
449  {
450  for (uint32_t i = 0; i < point.cov_points.size(); i++)
451  {
452  point.transformed_cov_points[i] = transform * point.cov_points[i];
453  }
454  }
455  point.transformed = true;
456  return true;
457  }
458  point.transformed = false;
459  return false;
460  }
461 
463  {
464  bool transformed = false;
465 
466  for (auto &pt : points_)
467  {
468  transformed = transformed | TransformPoint(pt);
469  }
470 
471  transformed = transformed | TransformPoint(cur_point_);
472  if (laps_.size() > 0)
473  {
474  for (auto &lap : laps_)
475  {
476  for (auto &pt : lap)
477  {
478  transformed = transformed | TransformPoint(pt);
479  }
480  }
481  }
482  if (!points_.empty() && !transformed)
483  {
484  PrintError("No transform between " + cur_point_.source_frame + " and " +
485  target_frame_);
486  }
487  }
488 
490  {
491  bool transformed = points_.size() != 0;
492  glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5);
493  glLineWidth(3);
494  QColor base_color = color_;
495 
496  for (size_t i = 0; i < laps_.size(); i++)
497  {
498  UpdateColor(base_color, static_cast<int>(i));
499  if (draw_style_ == LINES)
500  {
501  glLineWidth(3);
502  glBegin(GL_LINE_STRIP);
503  }
504  else
505  {
506  glPointSize(6);
507  glBegin(GL_POINTS);
508  }
509 
510  for (const auto& pt : laps_[i])
511  {
512  if (pt.transformed)
513  {
514  glVertex2d(pt.transformed_point.getX(),
515  pt.transformed_point.getY());
516  }
517  }
518  glEnd();
519  }
520 
521  if (draw_style_ == LINES)
522  {
523  glLineWidth(3);
524  glBegin(GL_LINE_STRIP);
525  }
526  else
527  {
528  glPointSize(6);
529  glBegin(GL_POINTS);
530  }
531 
532  glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(), 0.5);
533 
534  if (points_.size() > 0)
535  {
536  for (const auto &pt : points_)
537  {
538  transformed &= pt.transformed;
539  if (pt.transformed)
540  {
541  glVertex2d(pt.transformed_point.getX(),
542  pt.transformed_point.getY());
543  }
544  }
545  }
546 
547  glEnd();
548  return transformed;
549  }
550 
551  void PointDrawingPlugin::UpdateColor(QColor base_color, int i)
552  {
553  int hue = static_cast<int>(color_.hue() + (i + 1.0) * 10.0 * M_PI);
554  if (hue > 360)
555  {
556  hue %= 360;
557  }
558  int sat = color_.saturation();
559  int v = color_.value();
560  base_color.setHsv(hue, sat, v);
561  glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(),
562  0.5);
563  }
564 
566  {
567  glLineWidth(4);
568 
569  glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 1.0);
570 
572  {
573  for (const auto &pt : points_)
574  {
575  if (!pt.transformed || pt.transformed_cov_points.empty())
576  {
577  continue;
578  }
579  glBegin(GL_LINE_STRIP);
580 
581  for (uint32_t i = 0; i < pt.transformed_cov_points.size(); i++)
582  {
583  glVertex2d(pt.transformed_cov_points[i].getX(),
584  pt.transformed_cov_points[i].getY());
585  }
586 
587  glVertex2d(pt.transformed_cov_points.front().getX(),
588  pt.transformed_cov_points.front().getY());
589 
590  glEnd();
591  }
592  }
594  {
595  glBegin(GL_LINE_STRIP);
596 
597  for (uint32_t i = 0; i < cur_point_.transformed_cov_points.size(); i++)
598  {
599  glVertex2d(cur_point_.transformed_cov_points[i].getX(),
601  }
602 
603  glVertex2d(cur_point_.transformed_cov_points.front().getX(),
604  cur_point_.transformed_cov_points.front().getY());
605 
606  glEnd();
607  }
608  }
609 
611  {
612  bool success = laps_.size() != 0 && points_.size() != 0;
613  glColor4d(color_.redF(), color_.greenF(), color_.blueF(), 0.5);
614  glLineWidth(2);
615  QColor base_color = color_;
616  if (laps_.size() != 0)
617  {
618  for (size_t i = 0; i < laps_.size(); i++)
619  {
620  UpdateColor(base_color, static_cast<int>(i));
621  for (const auto &pt : laps_[i])
622  {
623  glBegin(GL_LINE_STRIP);
624  success &= DrawArrow(pt);
625  glEnd();
626  }
627  }
628  glEnd();
629 
630  int hue = static_cast<int>(color_.hue() + laps_.size() * 10.0 * M_PI);
631  int sat = color_.saturation();
632  int v = color_.value();
633  base_color.setHsv(hue, sat, v);
634  glColor4d(base_color.redF(), base_color.greenF(), base_color.blueF(),
635  0.5);
636  }
637 
638  if (points_.size() > 0)
639  {
640  for (const auto& pt : points_)
641  {
642  glBegin(GL_LINE_STRIP);
643  success &= DrawArrow(pt);
644  glEnd();
645  }
646  }
647 
648  return success;
649  }
650 }
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
ROSCPP_DECL bool check()
IconWidget * icon_
virtual void PositionToleranceChanged(double value)
virtual void SetArrowSize(int arrowSize)
virtual void UpdateColor(QColor base_color, int i)
std::vector< std::deque< StampedPoint > > laps_
std::string target_frame_
tf::Vector3 Point
void SetPixmap(QPixmap pixmap)
std::deque< StampedPoint > points_
#define ROS_INFO(...)
tf::Quaternion GetOrientation() const
void TargetFrameChanged(const std::string &target_frame)
virtual void SetColor(const QColor &color)
virtual void SetStaticArrowSizes(bool isChecked)
virtual bool DrawArrow(const StampedPoint &point)
virtual void ShowAllCovariancesToggled(bool checked)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
virtual void CovariancedToggled(bool checked)
virtual void SetDrawStyle(QString style)
const std::deque< StampedPoint > & points() const
virtual bool TransformPoint(StampedPoint &point)
virtual void PrintError(const std::string &message)=0


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:33