measuring_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2018, 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 #include <mapviz/mapviz_plugin.h>
32 
33 // QT libraries
34 #include <QDateTime>
35 #include <QMouseEvent>
36 #include <QTextStream>
37 #include <QPainter>
38 
39 #if QT_VERSION >= 0x050000
40 #include <QGuiApplication>
41 #else
42 #include <QApplication>
43 #endif
44 
45 // ROS Libraries
46 #include <ros/ros.h>
47 
48 // Mapviz Libraries
50 
53 
54 namespace mapviz_plugins
55 {
56 
58  config_widget_(new QWidget()),
59  selected_point_(-1),
60  is_mouse_down_(false),
61  max_ms_(Q_INT64_C(500)),
62  max_distance_(2.0),
63  map_canvas_(NULL)
64 {
65  ui_.setupUi(config_widget_);
66  ui_.main_color->setColor(Qt::black);
67  ui_.bkgnd_color->setColor(Qt::white);
68 
69  QObject::connect(ui_.clear, SIGNAL(clicked()), this,
70  SLOT(Clear()));
71  QObject::connect(ui_.show_measurements, SIGNAL(toggled(bool)), this,
72  SLOT(MeasurementsToggled(bool)));
73  QObject::connect(ui_.show_bkgnd_color, SIGNAL(toggled(bool)), this,
74  SLOT(BkgndColorToggled(bool)));
75  QObject::connect(ui_.font_size, SIGNAL(valueChanged(int)), this,
76  SLOT(FontSizeChanged(int)));
77  QObject::connect(ui_.alpha, SIGNAL(valueChanged(double)), this,
78  SLOT(AlphaChanged(double)));
79  connect(ui_.main_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon()));
80  connect(ui_.bkgnd_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(DrawIcon()));
81 
82 #if QT_VERSION >= 0x050000
83  ui_.measurement->setText(tr("Click on the map. Distance between clicks will appear here"));
84  ui_.totaldistance->setText(tr("Click on the map. Total distance between clicks will appear here"));
85 #endif
86 }
87 
89 {
90  if (map_canvas_)
91  {
92  map_canvas_->removeEventFilter(this);
93  }
94 }
95 
97 {
98  vertices_.clear();
99  measurements_.clear();
100  ui_.measurement->setText(tr("Click on the map. Distance between clicks will appear here"));
101  ui_.totaldistance->setText(tr("Click on the map. Total distance between clicks will appear here"));
102 }
103 
104 QWidget* MeasuringPlugin::GetConfigWidget(QWidget* parent)
105 {
106  config_widget_->setParent(parent);
107 
108  return config_widget_;
109 }
110 
111 bool MeasuringPlugin::Initialize(QGLWidget* canvas)
112 {
113  map_canvas_ = static_cast< mapviz::MapCanvas* >(canvas);
114  map_canvas_->installEventFilter(this);
115 
116  initialized_ = true;
117  PrintInfo("OK");
118 
119  return true;
120 }
121 
122 bool MeasuringPlugin::eventFilter(QObject* object, QEvent* event)
123 {
124  if(!this->Visible())
125  {
126  ROS_DEBUG("Ignoring mouse event, since measuring plugin is hidden");
127  return false;
128  }
129 
130  switch (event->type())
131  {
132  case QEvent::MouseButtonPress:
133  return handleMousePress(static_cast< QMouseEvent* >(event));
134  case QEvent::MouseButtonRelease:
135  return handleMouseRelease(static_cast< QMouseEvent* >(event));
136  case QEvent::MouseMove:
137  return handleMouseMove(static_cast< QMouseEvent* >(event));
138  default:
139  return false;
140  }
141 }
142 
143 bool MeasuringPlugin::handleMousePress(QMouseEvent* event)
144 {
145  selected_point_ = -1;
146  int closest_point = 0;
147  double closest_distance = std::numeric_limits<double>::max();
148 #if QT_VERSION >= 0x050000
149  QPointF point = event->localPos();
150 #else
151  QPointF point = event->posF();
152 #endif
153  ROS_DEBUG("Map point: %f %f", point.x(), point.y());
154  for (size_t i = 0; i < vertices_.size(); i++)
155  {
156  tf::Vector3 vertex = vertices_[i];
157  QPointF transformed = map_canvas_->FixedFrameToMapGlCoord(QPointF(vertex.x(), vertex.y()));
158 
159  double distance = QLineF(transformed, point).length();
160 
161  if (distance < closest_distance)
162  {
163  closest_distance = distance;
164  closest_point = static_cast<int>(i);
165  }
166  }
167  if (event->button() == Qt::LeftButton)
168  {
169  if (closest_distance < 15)
170  {
171  selected_point_ = closest_point;
172  return true;
173  }
174  else
175  {
176  is_mouse_down_ = true;
177 #if QT_VERSION >= 0x050000
178  mouse_down_pos_ = event->localPos();
179 #else
180  mouse_down_pos_ = event->posF();
181 #endif
182  mouse_down_time_ = QDateTime::currentMSecsSinceEpoch();
183  return false;
184  }
185  }
186  else if (event->button() == Qt::RightButton)
187  {
188  if (closest_distance < 15)
189  {
190  vertices_.erase(vertices_.begin() + closest_point);
191  DistanceCalculation(); //function to calculate distance
192  return true;
193  }
194  }
195 
196  // Let other plugins process this event too
197  return false;
198 }
199 
200 bool MeasuringPlugin::handleMouseRelease(QMouseEvent* event)
201 {
202  if (selected_point_ >= 0 && static_cast<size_t>(selected_point_) < vertices_.size())
203  {
204 #if QT_VERSION >= 0x050000
205  QPointF point = event->localPos();
206 #else
207  QPointF point = event->posF();
208 #endif
209  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
210  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
211  vertices_[selected_point_].setX(position.x());
212  vertices_[selected_point_].setY(position.y());
213 
215 
216  selected_point_ = -1;
217 
218  return true;
219  }
220  else if (is_mouse_down_)
221  {
222 #if QT_VERSION >= 0x050000
223  qreal distance = QLineF(mouse_down_pos_, event->localPos()).length();
224 #else
225  qreal distance = QLineF(mouse_down_pos_, event->posF()).length();
226 #endif
227  qint64 msecsDiff = QDateTime::currentMSecsSinceEpoch() - mouse_down_time_;
228 
229  // Only fire the event if the mouse has moved less than the maximum distance
230  // and was held for shorter than the maximum time.. This prevents click
231  // events from being fired if the user is dragging the mouse across the map
232  // or just holding the cursor in place.
233  if (msecsDiff < max_ms_ && distance <= max_distance_)
234  {
235 #if QT_VERSION >= 0x050000
236  QPointF point = event->localPos();
237 #else
238  QPointF point = event->posF();
239 #endif
240 
241  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
242  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
243  vertices_.push_back(position);
244  DistanceCalculation(); //call to calculate distance
245  }
246  }
247  is_mouse_down_ = false;
248  // Let other plugins process this event too
249  return false;
250 }
251 
253 {
254  double distance_instant = -1; //measurement between last two points
255  double distance_sum = 0; //sum of distance from all points
256  tf::Vector3 last_position_(0,0,0);
257  std::string frame = target_frame_;
258  measurements_.clear();
259  for (size_t i = 0; i < vertices_.size(); i++)
260  {
261  tf::Vector3 vertex = vertices_[i];
262  if (last_position_ != tf::Vector3(0,0,0))
263  {
264  distance_instant = last_position_.distance(vertex);
265  distance_sum = distance_sum + distance_instant;
266  measurements_.push_back(distance_instant);
267  }
268  last_position_ = vertex;
269  }
270  measurements_.push_back(distance_sum);
271 
272  QString new_point;
273  QTextStream stream(&new_point);
274  stream.setRealNumberPrecision(4);
275 
276  if (distance_instant > 0.0)
277  {
278  stream << distance_instant << " meters";
279  }
280 
281  ui_.measurement->setText(new_point);
282 
283  QString new_point2;
284  QTextStream stream2(&new_point2);
285  stream2.setRealNumberPrecision(4);
286 
287  if (distance_sum > 0.0)
288  {
289  stream2 << distance_sum << " meters";
290  }
291 
292  ui_.totaldistance->setText(new_point2);
293 }
294 
295 bool MeasuringPlugin::handleMouseMove(QMouseEvent* event)
296 {
297  if (selected_point_ >= 0 && static_cast<size_t>(selected_point_) < vertices_.size())
298  {
299 #if QT_VERSION >= 0x050000
300  QPointF point = event->localPos();
301 #else
302  QPointF point = event->posF();
303 #endif
304  std::string frame = target_frame_;
305  QPointF transformed = map_canvas_->MapGlCoordToFixedFrame(point);
306  tf::Vector3 position(transformed.x(), transformed.y(), 0.0);
307  vertices_[selected_point_].setY(position.y());
308  vertices_[selected_point_].setX(position.x());
309  DistanceCalculation(); //function to calculate distance
310  return true;
311  }// Let other plugins process this event too
312  return false;
313 }
314 
315 void MeasuringPlugin::Draw(double x, double y, double scale)
316 {
317  glLineWidth(1);
318  const QColor color = ui_.main_color->color();
319  glColor4d(color.redF(), color.greenF(), color.blueF(), ui_.alpha->value()/2.0);
320  glBegin(GL_LINE_STRIP);
321 
322  for (const auto& vertex: vertices_)
323  {
324  glVertex2d(vertex.x(), vertex.y());
325  }
326 
327  glEnd();
328 
329  glBegin(GL_LINES);
330 
331  glColor4d(color.redF(), color.greenF(), color.blueF(), ui_.alpha->value()/2.0);
332 
333  glEnd();
334 
335  // Draw vertices
336  glPointSize(9);
337  glBegin(GL_POINTS);
338 
339  for (const auto& vertex: vertices_)
340  {
341  glVertex2d(vertex.x(), vertex.y());
342  }
343  glEnd();
344 
345  PrintInfo("OK");
346 }
347 
348 void MeasuringPlugin::Paint(QPainter* painter, double x, double y, double scale)
349 {
350  bool show_measurements = ui_.show_measurements->isChecked();
351  if (!show_measurements || vertices_.empty())
352  {
353  return;
354  }
355 
356  QTransform tf = painter->worldTransform();
357  QFont font("Helvetica", ui_.font_size->value());
358  painter->setFont(font);
359  painter->save();
360  painter->resetTransform();
361 
362  //set the draw color for the text to be the same as the rest
363  QColor color = ui_.main_color->color();
364  double alpha = ui_.alpha->value()*2.0 < 1.0 ? ui_.alpha->value()*2.0 : 1.0;
365  color.setAlphaF(alpha);
366  QPen pen(QBrush(color), 1);
367  painter->setPen(pen);
368 
369  const QRectF qrect = QRectF(0, 0, 0, 0);
370  MeasurementBox mb;
371  std::vector<MeasurementBox> tags;
372 
373  //(midpoint positioned) measurements
374  for (int i=0; i<vertices_.size()-1; i++)
375  {
376  tf::Vector3 v1 = vertices_[i];
377  tf::Vector3 v2 = vertices_[i+1];
378 
379  mb.string.setNum(measurements_[i], 'g', 5);
380  mb.string.prepend(" ");
381  mb.string.append(" m ");
382  //drawText used here to get correct mb.rect size
383  painter->drawText(qrect, 0, mb.string, &mb.rect);
384  mb.rect.moveTopLeft(tf.map(QPointF((v1.x()+v2.x())/2, (v1.y()+v2.y())/2)));
385  tags.push_back(mb);
386  }
387  //(endpoint positioned) total dist
388  mb.string.setNum(measurements_.back(), 'g', 5);
389  mb.string.prepend(" Total: ");
390  mb.string.append(" m ");
391  painter->drawText(qrect, 0, mb.string, &mb.rect);
392  mb.rect.moveTopLeft(tf.map(QPointF(vertices_.back().x(), vertices_.back().y())));
393  tags.push_back(mb);
394 
395  //prevent text overlapping
396  for (int i=0; i<tags.size(); i++)
397  {
398  for (int j=0; j<tags.size(); j++)
399  {
400  if (i != j && tags[i].rect.intersects(tags[j].rect))
401  {
402  QRectF overlap = tags[i].rect.intersected(tags[j].rect);
403  if (tags[i].rect.y() > tags[j].rect.y())
404  {
405  tags[i].rect.moveTop(tags[i].rect.y() + overlap.height());
406  }
407  else
408  {
409  tags[i].rect.moveTop(tags[i].rect.y() - overlap.height());
410  }
411  }
412  }
413  }
414 
415  //paint tags
416  for (const auto& tag: tags)
417  {
418  if (ui_.show_bkgnd_color->isChecked())
419  {
420  color = ui_.bkgnd_color->color();
421  color.setAlphaF(ui_.alpha->value());
422  painter->fillRect(tag.rect, color);
423  painter->drawRect(tag.rect);
424  }
425  painter->drawText(tag.rect, tag.string);
426  }
427  painter->restore();
428 }
429 
430 void MeasuringPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
431 {
432  if (node["main_color"])
433  {
434  std::string color;
435  node["main_color"] >> color;
436  ui_.main_color->setColor(QColor(color.c_str()));
437  }
438 
439  if (node["bkgnd_color"])
440  {
441  std::string color;
442  node["bkgnd_color"] >> color;
443  ui_.bkgnd_color->setColor(QColor(color.c_str()));
444  }
445 
446  if (node["show_bkgnd_color"])
447  {
448  bool show_bkgnd_color = false;
449  node["show_bkgnd_color"] >> show_bkgnd_color;
450  ui_.show_bkgnd_color->setChecked(show_bkgnd_color);
451  BkgndColorToggled(show_bkgnd_color);
452  }
453 
454  if (node["show_measurements"])
455  {
456  bool show_measurements = false;
457  node["show_measurements"] >> show_measurements;
458  ui_.show_measurements->setChecked(show_measurements);
459  MeasurementsToggled(show_measurements);
460  }
461 
462  if (node["font_size"])
463  {
464  int font_size;
465  node["font_size"] >> font_size;
466  ui_.font_size->setValue(font_size);
467  FontSizeChanged(font_size);
468  }
469 
470  if (node["alpha"])
471  {
472  double alpha;
473  node["alpha"] >> alpha;
474  ui_.alpha->setValue(alpha);
475  AlphaChanged(alpha);
476  }
477 }
478 
479 void MeasuringPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
480 {
481  emitter << YAML::Key << "main_color" << YAML::Value << ui_.main_color->color().name().toStdString();
482  emitter << YAML::Key << "bkgnd_color" << YAML::Value << ui_.bkgnd_color->color().name().toStdString();
483  emitter << YAML::Key << "show_bkgnd_color" << YAML::Value << ui_.show_bkgnd_color->isChecked();
484  emitter << YAML::Key << "show_measurements" << YAML::Value << ui_.show_measurements->isChecked();
485  emitter << YAML::Key << "font_size" << YAML::Value << ui_.font_size->value();
486  emitter << YAML::Key << "alpha" << YAML::Value << ui_.alpha->value();
487 }
488 
489 void MeasuringPlugin::PrintError(const std::string& message)
490 {
491  PrintErrorHelper(ui_.status, message, 1.0);
492 }
493 
494 void MeasuringPlugin::PrintInfo(const std::string& message)
495 {
496  PrintInfoHelper(ui_.status, message, 1.0);
497 }
498 
499 void MeasuringPlugin::PrintWarning(const std::string& message)
500 {
501  PrintWarningHelper(ui_.status, message, 1.0);
502 }
503 
504 } // namespace mapviz_plugins
bool eventFilter(QObject *object, QEvent *event)
#define NULL
bool Initialize(QGLWidget *canvas)
QPointF MapGlCoordToFixedFrame(const QPointF &point)
void PrintWarning(const std::string &message)
void Paint(QPainter *painter, double x, double y, double scale)
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
bool handleMouseRelease(QMouseEvent *)
std::string target_frame_
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
std::vector< double > measurements_
bool Visible() const
QWidget * GetConfigWidget(QWidget *parent)
QPointF FixedFrameToMapGlCoord(const QPointF &point)
std::vector< tf::Vector3 > vertices_
void PrintError(const std::string &message)
virtual void DrawIcon()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void Draw(double x, double y, double scale)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void LoadConfig(const YAML::Node &node, const std::string &path)
void PrintInfo(const std::string &message)
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
#define ROS_DEBUG(...)


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