mapviz.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 #include <mapviz/mapviz.h>
31 
32 // C++ standard libraries
33 #include <cmath>
34 #include <cstdlib>
35 #include <algorithm>
36 #include <fstream>
37 #include <sstream>
38 
39 // Boost libraries
40 #include <boost/algorithm/string/replace.hpp>
41 #include <boost/algorithm/string/split.hpp>
42 #include <boost/algorithm/string/join.hpp>
43 #include <boost/date_time/posix_time/posix_time.hpp>
44 #include <boost/filesystem.hpp>
45 #include <boost/make_shared.hpp>
46 
47 // OpenCV libraries
48 #include <opencv2/core/core.hpp>
49 #include <opencv2/imgproc/imgproc.hpp>
50 
51 #if CV_MAJOR_VERSION > 2
52 #include <opencv2/imgcodecs/imgcodecs.hpp>
53 #include <opencv2/videoio/videoio.hpp>
54 #endif
55 
56 // QT libraries
57 #if QT_VERSION >= 0x050000
58 #include <QtWidgets/QApplication>
59 #else
60 #include <QtGui/QApplication>
61 #endif
62 #include <QFileDialog>
63 #include <QActionGroup>
64 #include <QColorDialog>
65 #include <QLabel>
66 #include <QMessageBox>
67 #include <QProcessEnvironment>
68 #include <QFileInfo>
69 #include <QListWidgetItem>
70 #include <QMutexLocker>
71 
75 
76 #include <mapviz/config_item.h>
77 #include <QtGui/QtGui>
78 
80 
81 namespace mapviz
82 {
83 const QString Mapviz::ROS_WORKSPACE_VAR = "ROS_WORKSPACE";
84 const QString Mapviz::MAPVIZ_CONFIG_FILE = "/.mapviz_config";
85 const std::string Mapviz::IMAGE_TRANSPORT_PARAM = "image_transport";
86 
87 Mapviz::Mapviz(bool is_standalone, int argc, char** argv, QWidget *parent, Qt::WindowFlags flags) :
88  QMainWindow(parent, flags),
89  xy_pos_label_(new QLabel("fixed: 0.0,0.0")),
90  lat_lon_pos_label_(new QLabel("lat/lon: 0.0,0.0")),
91  argc_(argc),
92  argv_(argv),
93  is_standalone_(is_standalone),
94  initialized_(false),
95  force_720p_(false),
96  force_480p_(false),
97  resizable_(true),
98  background_(Qt::gray),
99  capture_directory_("~"),
100  vid_writer_(NULL),
101  updating_frames_(false),
102  node_(NULL),
103  canvas_(NULL)
104 {
105  ui_.setupUi(this);
106 
107  xy_pos_label_->setVisible(false);
108  lat_lon_pos_label_->setVisible(false);
109 
110  ui_.statusbar->addPermanentWidget(xy_pos_label_);
111  ui_.statusbar->addPermanentWidget(lat_lon_pos_label_);
112 
113  spacer1_ = new QWidget(ui_.statusbar);
114  spacer1_->setMaximumSize(22,22);
115  spacer1_->setMinimumSize(22,22);
116  ui_.statusbar->addPermanentWidget(spacer1_);
117 
118  screenshot_button_ = new QPushButton();
119  screenshot_button_->setMinimumSize(22, 22);
120  screenshot_button_->setMaximumSize(22,22);
121  screenshot_button_->setIcon(QIcon(":/images/image-x-generic.png"));
122  screenshot_button_->setFlat(true);
123  screenshot_button_->setToolTip("Capture screenshot of display canvas");
124  ui_.statusbar->addPermanentWidget(screenshot_button_);
125 
126  spacer2_ = new QWidget(ui_.statusbar);
127  spacer2_->setMaximumSize(22,22);
128  spacer2_->setMinimumSize(22,22);
129  ui_.statusbar->addPermanentWidget(spacer2_);
130 
131  rec_button_ = new QPushButton();
132  rec_button_->setMinimumSize(22, 22);
133  rec_button_->setMaximumSize(22,22);
134  rec_button_->setIcon(QIcon(":/images/media-record.png"));
135  rec_button_->setCheckable(true);
136  rec_button_->setFlat(true);
137  rec_button_->setToolTip("Start recording video of display canvas");
138  ui_.statusbar->addPermanentWidget(rec_button_);
139 
140  stop_button_ = new QPushButton();
141  stop_button_->setMinimumSize(22, 22);
142  stop_button_->setMaximumSize(22,22);
143  stop_button_->setIcon(QIcon(":/images/media-playback-stop.png"));
144  stop_button_->setToolTip("Stop recording video of display canvas");
145  stop_button_->setEnabled(false);
146  stop_button_->setFlat(true);
147  ui_.statusbar->addPermanentWidget(stop_button_);
148 
149  spacer3_ = new QWidget(ui_.statusbar);
150  spacer3_->setMaximumSize(22,22);
151  spacer3_->setMinimumSize(22,22);
152  ui_.statusbar->addPermanentWidget(spacer3_);
153 
154  recenter_button_ = new QPushButton();
155  recenter_button_->setMinimumSize(22, 22);
156  recenter_button_->setMaximumSize(22, 22);
157  recenter_button_->setIcon(QIcon(":/images/arrow_in.png"));
158  recenter_button_->setToolTip("Reset the viewport to the default location and zoom level");
159  recenter_button_->setFlat(true);
160  ui_.statusbar->addPermanentWidget(recenter_button_);
161 
162  ui_.statusbar->setVisible(true);
163 
164  QActionGroup* group = new QActionGroup(this);
165 
166  ui_.actionForce_720p->setActionGroup(group);
167  ui_.actionForce_480p->setActionGroup(group);
168  ui_.actionResizable->setActionGroup(group);
169 
170  ui_.targetframe->addItem("<none>");
171 
172  canvas_ = new MapCanvas(this);
173  setCentralWidget(canvas_);
174 
175  connect(canvas_, SIGNAL(Hover(double,double,double)), this, SLOT(Hover(double,double,double)));
176  connect(ui_.configs, SIGNAL(ItemsMoved()), this, SLOT(ReorderDisplays()));
177  connect(ui_.actionExit, SIGNAL(triggered()), this, SLOT(close()));
178  connect(ui_.actionClear, SIGNAL(triggered()), this, SLOT(ClearConfig()));
179  connect(ui_.bg_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(SelectBackgroundColor(const QColor &)));
180 
181  connect(recenter_button_, SIGNAL(clicked()), this, SLOT(Recenter()));
182  connect(rec_button_, SIGNAL(toggled(bool)), this, SLOT(ToggleRecord(bool)));
183  connect(stop_button_, SIGNAL(clicked()), this, SLOT(StopRecord()));
184  connect(screenshot_button_, SIGNAL(clicked()), this, SLOT(Screenshot()));
185  connect(ui_.actionClear_History, SIGNAL(triggered()), this, SLOT(ClearHistory()));
186 
187  // Use a separate thread for writing video files so that it won't cause
188  // lag on the main thread.
189  // It's ok for the video writer to be a pointer that we intantiate here and
190  // then forget about; the worker thread will delete it when the thread exits.
191  vid_writer_ = new VideoWriter();
192  vid_writer_->moveToThread(&video_thread_);
193  connect(&video_thread_, SIGNAL(finished()), vid_writer_, SLOT(deleteLater()));
194  connect(this, SIGNAL(FrameGrabbed(QImage)), vid_writer_, SLOT(processFrame(QImage)));
195  video_thread_.start();
196 
197  image_transport_menu_ = new QMenu("Default Image Transport", ui_.menu_View);
198  ui_.menu_View->addMenu(image_transport_menu_);
199 
200  connect(image_transport_menu_, SIGNAL(aboutToShow()), this, SLOT(UpdateImageTransportMenu()));
201 
202  ui_.bg_color->setColor(background_);
204 }
205 
207 {
208  video_thread_.quit();
209  video_thread_.wait();
210  delete node_;
211 }
212 
213 void Mapviz::showEvent(QShowEvent* event)
214 {
215  Initialize();
216 }
217 
218 void Mapviz::closeEvent(QCloseEvent* event)
219 {
220  AutoSave();
221 
222  for (auto& display: plugins_)
223  {
224  MapvizPluginPtr plugin = display.second;
225  canvas_->RemovePlugin(plugin);
226  }
227 
228  plugins_.clear();
229 }
230 
232 {
233  if (!initialized_)
234  {
235  if (is_standalone_)
236  {
237  // If this Mapviz is running as a standalone application, it needs to init
238  // ROS and start spinning. If it's running as an rqt plugin, rqt will
239  // take care of that.
241 
242  spin_timer_.start(30);
243  connect(&spin_timer_, SIGNAL(timeout()), this, SLOT(SpinOnce()));
244  }
245 
246  node_ = new ros::NodeHandle("~");
247 
248  // Create a sub-menu that lists all available Image Transports
250  std::vector<std::string> transports = it.getLoadableTransports();
251  QActionGroup* group = new QActionGroup(image_transport_menu_);
252  for (std::vector<std::string>::iterator iter = transports.begin(); iter != transports.end(); iter++)
253  {
254  QString transport = QString::fromStdString(*iter).replace(
255  QString::fromStdString(IMAGE_TRANSPORT_PARAM) + "/", "");
256  QAction* action = image_transport_menu_->addAction(transport);
257  action->setCheckable(true);
258  group->addAction(action);
259  }
260 
261  connect(group, SIGNAL(triggered(QAction*)), this, SLOT(SetImageTransport(QAction*)));
262 
263  tf_ = boost::make_shared<tf::TransformListener>();
264  tf_manager_ = boost::make_shared<swri_transform_util::TransformManager>();
265  tf_manager_->Initialize(tf_);
266 
268  "mapviz", "mapviz::MapvizPlugin");
269 
270  std::vector<std::string> plugins = loader_->getDeclaredClasses();
271  for (unsigned int i = 0; i < plugins.size(); i++)
272  {
273  ROS_INFO("Found mapviz plugin: %s", plugins[i].c_str());
274  }
275 
277  canvas_->SetFixedFrame(ui_.fixedframe->currentText().toStdString());
278  canvas_->SetTargetFrame(ui_.targetframe->currentText().toStdString());
279 
280  ros::NodeHandle priv("~");
281 
282  add_display_srv_ = node_->advertiseService("add_mapviz_display", &Mapviz::AddDisplay, this);
283 
284  QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
285  QString default_path = QDir::homePath();
286  if (env.contains(ROS_WORKSPACE_VAR))
287  {
288  // If the ROS_WORKSPACE environment variable is defined, try to read our
289  // config file out of that. If we can't read it, fall back to trying to
290  // read one from the user's home directory.
291  QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
292  if (QFileInfo(ws_path + MAPVIZ_CONFIG_FILE).isReadable())
293  {
294  default_path = ws_path;
295  }
296  else
297  {
298  ROS_WARN("Could not load config file from ROS_WORKSPACE at %s; trying home directory...",
299  ws_path.toStdString().c_str());
300  }
301  }
302  default_path += MAPVIZ_CONFIG_FILE;
303 
304 
305  std::string config;
306  priv.param("config", config, default_path.toStdString());
307 
308  bool auto_save;
309  priv.param("auto_save_backup", auto_save, true);
310 
311  Open(config);
312 
313  UpdateFrames();
314  frame_timer_.start(1000);
315  connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(UpdateFrames()));
316 
317  if (auto_save)
318  {
319  save_timer_.start(10000);
320  connect(&save_timer_, SIGNAL(timeout()), this, SLOT(AutoSave()));
321  }
322 
323  connect(&record_timer_, SIGNAL(timeout()), this, SLOT(CaptureVideoFrame()));
324 
325  bool print_profile_data;
326  priv.param("print_profile_data", print_profile_data, false);
327  if (print_profile_data)
328  {
329  profile_timer_.start(2000);
330  connect(&profile_timer_, SIGNAL(timeout()), this, SLOT(HandleProfileTimer()));
331  }
332 
333  setFocus(); // Set the main window as focused object, prevent other fields from obtaining focus at startup
334 
335  initialized_ = true;
336  }
337 }
338 
340 {
341  if (ros::ok())
342  {
343  meas_spin_.start();
344  ros::spinOnce();
345  meas_spin_.stop();
346  }
347  else
348  {
349  QApplication::exit();
350  }
351 }
352 
354 {
355  std::vector<std::string> frames;
356  tf_->getFrameStrings(frames);
357  std::sort(frames.begin(), frames.end());
358 
359  if (ui_.fixedframe->count() >= 0 &&
360  static_cast<size_t>(ui_.fixedframe->count()) == frames.size())
361  {
362  bool changed = false;
363  for (size_t i = 0; i < frames.size(); i++)
364  {
365  if (frames[i] != ui_.fixedframe->itemText(i).toStdString())
366  {
367  changed = true;
368  }
369  }
370 
371  if (!changed)
372  return;
373  }
374 
375  updating_frames_ = true;
376 
377  std::string current_fixed = ui_.fixedframe->currentText().toStdString();
378 
379  ui_.fixedframe->clear();
380  for (size_t i = 0; i < frames.size(); i++)
381  {
382  ui_.fixedframe->addItem(frames[i].c_str());
383  }
384 
385  if (current_fixed != "")
386  {
387  int index = ui_.fixedframe->findText(current_fixed.c_str());
388  if (index < 0)
389  {
390  ui_.fixedframe->addItem(current_fixed.c_str());
391  }
392 
393  index = ui_.fixedframe->findText(current_fixed.c_str());
394  ui_.fixedframe->setCurrentIndex(index);
395  }
396 
397  std::string current_target = ui_.targetframe->currentText().toStdString();
398 
399  ui_.targetframe->clear();
400  ui_.targetframe->addItem("<none>");
401  for (size_t i = 0; i < frames.size(); i++)
402  {
403  ui_.targetframe->addItem(frames[i].c_str());
404  }
405 
406  if (current_target != "")
407  {
408  int index = ui_.targetframe->findText(current_target.c_str());
409  if (index < 0)
410  {
411  ui_.targetframe->addItem(current_target.c_str());
412  }
413 
414  index = ui_.targetframe->findText(current_target.c_str());
415  ui_.targetframe->setCurrentIndex(index);
416  }
417 
418  updating_frames_ = false;
419 
420  if (current_target != ui_.targetframe->currentText().toStdString())
421  {
422  TargetFrameSelected(ui_.targetframe->currentText());
423  }
424 
425  if (current_fixed != ui_.fixedframe->currentText().toStdString())
426  {
427  FixedFrameSelected(ui_.fixedframe->currentText());
428  }
429 }
430 
431 void Mapviz::Force720p(bool on)
432 {
433  if (force_720p_ != on)
434  {
435  force_720p_ = on;
436 
437  if (force_720p_)
438  {
439  force_480p_ = false;
440  resizable_ = false;
441  }
442 
444  }
445 }
446 
447 void Mapviz::Force480p(bool on)
448 {
449  if (force_480p_ != on)
450  {
451  force_480p_ = on;
452 
453  if (force_480p_)
454  {
455  force_720p_ = false;
456  resizable_ = false;
457  }
458 
460  }
461 }
462 
463 void Mapviz::SetResizable(bool on)
464 {
465  if (resizable_ != on)
466  {
467  resizable_ = on;
468 
469  if (resizable_)
470  {
471  force_720p_ = false;
472  force_480p_ = false;
473  }
474 
476  }
477 }
478 
480 {
481  canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred));
482  setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred));
483 
484  this->setMinimumSize(QSize(100, 100));
485  this->setMaximumSize(QSize(10000, 10000));
486 
487  if (force_720p_)
488  {
489  canvas_->setMinimumSize(1280, 720);
490  canvas_->setMaximumSize(1280, 720);
491  canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
492  adjustSize();
493  this->setMaximumSize(this->sizeHint());
494  this->setMinimumSize(this->sizeHint());
495  setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
496  }
497  else if (force_480p_)
498  {
499  canvas_->setMinimumSize(640, 480);
500  canvas_->setMaximumSize(640, 480);
501  canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
502  adjustSize();
503  this->setMaximumSize(this->sizeHint());
504  this->setMinimumSize(this->sizeHint());
505  setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
506  }
507  else if (stop_button_->isEnabled())
508  {
509  canvas_->setMinimumSize(canvas_->width(), canvas_->height());
510  canvas_->setMaximumSize(canvas_->width(), canvas_->height());
511  canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
512  adjustSize();
513  this->setMaximumSize(this->sizeHint());
514  this->setMinimumSize(this->sizeHint());
515  setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
516  }
517  else
518  {
519  canvas_->setMinimumSize(100, 100);
520  canvas_->setMaximumSize(10000, 10000);
521  }
522 }
523 
524 void Mapviz::Open(const std::string& filename)
525 {
526  ROS_INFO("Loading configuration from: %s", filename.c_str());
527  save_location_ = filename;
528 
529  std::string title;
530  size_t last_slash = filename.find_last_of('/');
531  if (last_slash != std::string::npos && last_slash != filename.size() - 1)
532  {
533  title = filename.substr(last_slash + 1) + " (" +
534  filename.substr(0, last_slash + 1) + ")";
535  }
536  else
537  {
538  title = filename;
539  }
540 
541  title += " - mapviz";
542  setWindowTitle(QString::fromStdString(title));
543 
544  YAML::Node doc;
545  if (!swri_yaml_util::LoadFile(filename, doc))
546  {
547  ROS_ERROR("Failed to load file: %s", filename.c_str());
548  return;
549  }
550 
551  std::vector<std::string> failed_plugins;
552 
553  try
554  {
555  boost::filesystem::path filepath(filename);
556  std::string config_path = filepath.parent_path().string();
557 
558  ClearDisplays();
559 
560  if (swri_yaml_util::FindValue(doc, "capture_directory"))
561  {
562  doc["capture_directory"] >> capture_directory_;
563  }
564 
565  if (swri_yaml_util::FindValue(doc, "fixed_frame"))
566  {
567  std::string fixed_frame;
568  doc["fixed_frame"] >> fixed_frame;
569  ui_.fixedframe->setEditText(fixed_frame.c_str());
570  }
571 
572  if (swri_yaml_util::FindValue(doc, "target_frame"))
573  {
574  std::string target_frame;
575  doc["target_frame"] >> target_frame;
576  ui_.targetframe->setEditText(target_frame.c_str());
577  }
578 
579  if (swri_yaml_util::FindValue(doc, "fix_orientation"))
580  {
581  bool fix_orientation = false;
582  doc["fix_orientation"] >> fix_orientation;
583  ui_.actionFix_Orientation->setChecked(fix_orientation);
584  }
585 
586  if (swri_yaml_util::FindValue(doc, "rotate_90"))
587  {
588  bool rotate_90 = false;
589  doc["rotate_90"] >> rotate_90;
590  ui_.actionRotate_90->setChecked(rotate_90);
591  }
592 
593  if (swri_yaml_util::FindValue(doc, "enable_antialiasing"))
594  {
595  bool enable_antialiasing = true;
596  doc["enable_antialiasing"] >> enable_antialiasing;
597  ui_.actionEnable_Antialiasing->setChecked(enable_antialiasing);
598  }
599 
600  if (swri_yaml_util::FindValue(doc, "show_displays"))
601  {
602  bool show_displays = false;
603  doc["show_displays"] >> show_displays;
604  ui_.actionConfig_Dock->setChecked(show_displays);
605  }
606 
607  if (swri_yaml_util::FindValue(doc, "show_capture_tools"))
608  {
609  bool show_capture_tools = false;
610  doc["show_capture_tools"] >> show_capture_tools;
611  ui_.actionShow_Capture_Tools->setChecked(show_capture_tools);
612  }
613 
614  if (swri_yaml_util::FindValue(doc, "show_status_bar"))
615  {
616  bool show_status_bar = false;
617  doc["show_status_bar"] >> show_status_bar;
618  ui_.actionShow_Status_Bar->setChecked(show_status_bar);
619  }
620 
621  if (swri_yaml_util::FindValue(doc, "show_capture_tools"))
622  {
623  bool show_capture_tools = false;
624  doc["show_capture_tools"] >> show_capture_tools;
625  ui_.actionShow_Capture_Tools->setChecked(show_capture_tools);
626  }
627 
628  if (swri_yaml_util::FindValue(doc, "window_width"))
629  {
630  int window_width = 0;
631  doc["window_width"] >> window_width;
632  resize(window_width, height());
633  }
634 
635  if (swri_yaml_util::FindValue(doc, "window_height"))
636  {
637  int window_height = 0;
638  doc["window_height"] >> window_height;
639  resize(width(), window_height);
640  }
641 
642  if (swri_yaml_util::FindValue(doc, "view_scale"))
643  {
644  float scale = 0;
645  doc["view_scale"] >> scale;
646  canvas_->SetViewScale(scale);
647  }
648 
649  if (swri_yaml_util::FindValue(doc, "offset_x"))
650  {
651  float x = 0;
652  doc["offset_x"] >> x;
653  canvas_->SetOffsetX(x);
654  }
655 
656  if (swri_yaml_util::FindValue(doc, "offset_x"))
657  {
658  float y = 0;
659  doc["offset_y"] >> y;
660  canvas_->SetOffsetY(y);
661  }
662 
663  if (swri_yaml_util::FindValue(doc, "force_720p"))
664  {
665  bool force_720p;
666  doc["force_720p"] >> force_720p;
667 
668  if (force_720p)
669  {
670  ui_.actionForce_720p->setChecked(true);
671  }
672  }
673 
674  if (swri_yaml_util::FindValue(doc, "force_480p"))
675  {
676  bool force_480p;
677  doc["force_480p"] >> force_480p;
678 
679  if (force_480p)
680  {
681  ui_.actionForce_480p->setChecked(true);
682  }
683  }
684 
686  {
687  std::string image_transport;
689 
691  }
692 
693  if (swri_yaml_util::FindValue(doc, "background"))
694  {
695  std::string color;
696  doc["background"] >> color;
697  background_ = QColor(color.c_str());
698  ui_.bg_color->setColor(background_);
700  }
701 
702  if (swri_yaml_util::FindValue(doc, "displays"))
703  {
704  const YAML::Node& displays = doc["displays"];
705  for (uint32_t i = 0; i < displays.size(); i++)
706  {
707  std::string type, name;
708  displays[i]["type"] >> type;
709  displays[i]["name"] >> name;
710 
711  const YAML::Node& config = displays[i]["config"];
712 
713  bool visible = false;
714  config["visible"] >> visible;
715 
716  bool collapsed = false;
717  config["collapsed"] >> collapsed;
718 
719  try
720  {
721  MapvizPluginPtr plugin =
722  CreateNewDisplay(name, type, visible, collapsed);
723  plugin->LoadConfig(config, config_path);
724  plugin->DrawIcon();
725  }
726  catch (const pluginlib::LibraryLoadException& e)
727  {
728  failed_plugins.push_back(type);
729  ROS_ERROR("%s", e.what());
730  }
731  }
732  }
733  }
734  catch (const YAML::ParserException& e)
735  {
736  ROS_ERROR("%s", e.what());
737  return;
738  }
739  catch (const YAML::Exception& e)
740  {
741  ROS_ERROR("%s", e.what());
742  return;
743  }
744 
745  if (!failed_plugins.empty())
746  {
747  std::stringstream message;
748  message << "The following plugin(s) failed to load:" << std::endl;
749  std::string failures = boost::algorithm::join(failed_plugins, "\n");
750  message << failures << std::endl << std::endl << "Check the ROS log for more details.";
751 
752  QMessageBox::warning(this, "Failed to load plugins", QString::fromStdString(message.str()));
753  }
754 }
755 
756 void Mapviz::Save(const std::string& filename)
757 {
758  std::ofstream fout(filename.c_str());
759  if (fout.fail())
760  {
761  ROS_ERROR("Failed to open file: %s", filename.c_str());
762  return;
763  }
764 
765  boost::filesystem::path filepath(filename);
766  std::string config_path = filepath.parent_path().string();
767 
768  YAML::Emitter out;
769 
770  out << YAML::BeginMap;
771  out << YAML::Key << "capture_directory" << YAML::Value << capture_directory_;
772  out << YAML::Key << "fixed_frame" << YAML::Value << ui_.fixedframe->currentText().toStdString();
773  out << YAML::Key << "target_frame" << YAML::Value << ui_.targetframe->currentText().toStdString();
774  out << YAML::Key << "fix_orientation" << YAML::Value << ui_.actionFix_Orientation->isChecked();
775  out << YAML::Key << "rotate_90" << YAML::Value << ui_.actionRotate_90->isChecked();
776  out << YAML::Key << "enable_antialiasing" << YAML::Value << ui_.actionEnable_Antialiasing->isChecked();
777  out << YAML::Key << "show_displays" << YAML::Value << ui_.actionConfig_Dock->isChecked();
778  out << YAML::Key << "show_status_bar" << YAML::Value << ui_.actionShow_Status_Bar->isChecked();
779  out << YAML::Key << "show_capture_tools" << YAML::Value << ui_.actionShow_Capture_Tools->isChecked();
780  out << YAML::Key << "window_width" << YAML::Value << width();
781  out << YAML::Key << "window_height" << YAML::Value << height();
782  out << YAML::Key << "view_scale" << YAML::Value << canvas_->ViewScale();
783  out << YAML::Key << "offset_x" << YAML::Value << canvas_->OffsetX();
784  out << YAML::Key << "offset_y" << YAML::Value << canvas_->OffsetY();
785  out << YAML::Key << "background" << YAML::Value << background_.name().toStdString();
786  std::string image_transport;
788  {
789  out << YAML::Key << IMAGE_TRANSPORT_PARAM << YAML::Value << image_transport;
790  }
791 
792  if (force_720p_)
793  {
794  out << YAML::Key << "force_720p" << YAML::Value << force_720p_;
795  }
796 
797  if (force_480p_)
798  {
799  out << YAML::Key << "force_480p" << YAML::Value << force_480p_;
800  }
801 
802  if (ui_.configs->count() > 0)
803  {
804  out << YAML::Key << "displays"<< YAML::Value << YAML::BeginSeq;
805 
806  for (int i = 0; i < ui_.configs->count(); i++)
807  {
808  out << YAML::BeginMap;
809  out << YAML::Key << "type" << YAML::Value << plugins_[ui_.configs->item(i)]->Type();
810  out << YAML::Key << "name" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Name().toStdString();
811  out << YAML::Key << "config" << YAML::Value;
812  out << YAML::BeginMap;
813 
814  out << YAML::Key << "visible" << YAML::Value << plugins_[ui_.configs->item(i)]->Visible();
815  out << YAML::Key << "collapsed" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Collapsed();
816 
817  plugins_[ui_.configs->item(i)]->SaveConfig(out, config_path);
818 
819  out << YAML::EndMap;
820  out << YAML::EndMap;
821  }
822 
823  out << YAML::EndSeq;
824  }
825 
826  out << YAML::EndMap;
827 
828  fout << out.c_str();
829  fout.close();
830 }
831 
833 {
834  QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
835  QString default_path = QDir::homePath();
836 
837  if (env.contains(ROS_WORKSPACE_VAR))
838  {
839  // Try to save our config in the ROS_WORKSPACE directory, but if we can't write
840  // to that -- probably because it is read-only -- try to use the home directory
841  // instead.
842  QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
843  QString ws_file = ws_path + MAPVIZ_CONFIG_FILE;
844  QFileInfo file_info(ws_file);
845  QFileInfo dir_info(ws_path);
846  if ((!file_info.exists() && dir_info.isWritable()) ||
847  file_info.isWritable())
848  {
849  // Note that FileInfo::isWritable will return false if a file does not exist, so
850  // we need to check both if the target file is writable and if the target dir is
851  // writable if the file doesn't exist.
852  default_path = ws_path;
853  }
854  else
855  {
856  ROS_WARN("Could not write config file to %s. Trying home directory.",
857  (ws_path + MAPVIZ_CONFIG_FILE).toStdString().c_str());
858  }
859  }
860  default_path += MAPVIZ_CONFIG_FILE;
861 
862 
863  Save(default_path.toStdString());
864 }
865 
867 {
868  QFileDialog dialog(this, "Select Config File");
869  dialog.setFileMode(QFileDialog::ExistingFile);
870  dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
871  std::string directory = save_location_.substr(0, save_location_.find_last_of('/') + 1);
872  dialog.setDirectory(QString(directory.c_str()));
873 
874  dialog.exec();
875 
876  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
877  {
878  std::string path = dialog.selectedFiles().first().toStdString();
879  Open(path);
880  }
881 }
882 
884 {
885  ClearDisplays();
886 }
887 
889 {
890  if (save_location_.length() == 0)
891  {
892  SaveConfigAs();
893  return;
894  }
895 
897 }
898 
900 {
901  QFileDialog dialog(this, "Save Config File");
902  dialog.setFileMode(QFileDialog::AnyFile);
903  dialog.setAcceptMode(QFileDialog::AcceptSave);
904  dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
905  dialog.setDefaultSuffix("mvc");
906  std::string directory = save_location_.substr(0, save_location_.find_last_of('/') + 1);
907  dialog.setDirectory(QString(directory.c_str()));
908  dialog.exec();
909 
910  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
911  {
912  std::string path = dialog.selectedFiles().first().toStdString();
913 
914  std::string title;
915  size_t last_slash = path.find_last_of('/');
916  if (last_slash != std::string::npos && last_slash != path.size() - 1)
917  {
918  title = path.substr(last_slash + 1) + " (" +
919  path.substr(0, last_slash + 1) + ")";
920  }
921  else
922  {
923  title = path;
924  }
925  title += " - mapviz";
926  setWindowTitle(QString::fromStdString(title));
927  Save(path);
928  save_location_ = path;
929  }
930 }
931 
933 {
934  ROS_DEBUG("Mapviz::ClearHistory()");
935  for (auto& plugin: plugins_)
936  {
937  plugin.second->ClearHistory();
938  }
939 }
940 
942 {
943  ROS_INFO("Select new display ...");
944  QDialog dialog;
945  Ui::pluginselect ui;
946  ui.setupUi(&dialog);
947 
948  std::vector<std::string> plugins = loader_->getDeclaredClasses();
949  std::map<std::string, std::string> plugin_types;
950  for (size_t i = 0; i < plugins.size(); i++)
951  {
952  QString type(plugins[i].c_str());
953  type = type.split('/').last();
954  ui.displaylist->addItem(type);
955  plugin_types[type.toStdString()] = plugins[i];
956  }
957  ui.displaylist->setCurrentRow(0);
958 
959  dialog.exec();
960 
961  if (dialog.result() == QDialog::Accepted)
962  {
963  std::string type_name = ui.displaylist->selectedItems().first()->text().toStdString();
964  std::string type = plugin_types[type_name];
965  std::string name = "new display";
966  try
967  {
968  CreateNewDisplay(name, type, true, false);
969  }
970  catch (const pluginlib::LibraryLoadException& e)
971  {
972  std::stringstream message;
973  message << "Unable to load " << type << "." << std::endl << "Check the ROS log for more details.";
974  QMessageBox::warning(this, "Plugin failed to load", QString::fromStdString(message.str()));
975  ROS_ERROR("%s", e.what());
976  }
977  }
978 }
979 
981  AddMapvizDisplay::Request& req,
982  AddMapvizDisplay::Response& resp)
983 {
984  std::map<std::string, std::string> properties;
985  for (auto& property: req.properties)
986  {
987  properties[property.key] = property.value;
988  }
989 
990  YAML::Node config;
991  if (!swri_yaml_util::LoadMap(properties, config))
992  {
993  ROS_ERROR("Failed to parse properties into YAML.");
994  return false;
995  }
996 
997  for (auto& display: plugins_)
998  {
999  MapvizPluginPtr plugin = display.second;
1000  if (!plugin)
1001  {
1002  ROS_ERROR("Invalid plugin ptr.");
1003  continue;
1004  }
1005  if (plugin->Name() == req.name && plugin->Type() ==req.type)
1006  {
1007  plugin->LoadConfig(config, "");
1008  plugin->SetVisible(req.visible);
1009 
1010  if (req.draw_order > 0)
1011  {
1012  display.first->setData(Qt::UserRole, QVariant(req.draw_order - 1.1));
1013  ui_.configs->sortItems();
1014 
1015  ReorderDisplays();
1016  }
1017  else if (req.draw_order < 0)
1018  {
1019  display.first->setData(Qt::UserRole, QVariant(ui_.configs->count() + req.draw_order + 0.1));
1020  ui_.configs->sortItems();
1021 
1022  ReorderDisplays();
1023  }
1024 
1025  resp.success = true;
1026 
1027  return true;
1028  }
1029  }
1030 
1031  try
1032  {
1033  MapvizPluginPtr plugin =
1034  CreateNewDisplay(req.name, req.type, req.visible, false, req.draw_order);
1035  plugin->LoadConfig(config, "");
1036  plugin->DrawIcon();
1037  resp.success = true;
1038  }
1039  catch (const pluginlib::LibraryLoadException& e)
1040  {
1041  ROS_ERROR("%s", e.what());
1042  resp.success = false;
1043  resp.message = "Failed to load display plug-in.";
1044  }
1045 
1046  return true;
1047 }
1048 
1049 void Mapviz::Hover(double x, double y, double scale)
1050 {
1051  if (ui_.statusbar->isVisible())
1052  {
1053  if (scale == 0)
1054  {
1055  xy_pos_label_->setVisible(false);
1056  lat_lon_pos_label_->setVisible(false);
1057  return;
1058  }
1059 
1060  int32_t precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / scale))));
1061 
1062  QString text = ui_.fixedframe->currentText();
1063  if (text.isEmpty() || text == "/")
1064  {
1065  text = "fixed";
1066  }
1067  text += ": ";
1068 
1069  std::ostringstream x_ss;
1070  x_ss << std::fixed << std::setprecision(precision);
1071  x_ss << x;
1072  text += x_ss.str().c_str();
1073 
1074  text += ", ";
1075 
1076  std::ostringstream y_ss;
1077  y_ss << std::fixed << std::setprecision(precision);
1078  y_ss << y;
1079  text += y_ss.str().c_str();
1080 
1081  xy_pos_label_->setText(text);
1082  xy_pos_label_->setVisible(true);
1083  xy_pos_label_->update();
1084 
1086  if (tf_manager_->SupportsTransform(
1088  ui_.fixedframe->currentText().toStdString()) &&
1089  tf_manager_->GetTransform(
1091  ui_.fixedframe->currentText().toStdString(),
1092  transform))
1093  {
1094  tf::Vector3 point(x, y, 0);
1095  point = transform * point;
1096 
1097  QString lat_lon_text = "lat/lon: ";
1098 
1099  double lat_scale = (1.0 / 111111.0) * scale;
1100  int32_t lat_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lat_scale))));
1101 
1102  std::ostringstream lat_ss;
1103  lat_ss << std::fixed << std::setprecision(lat_precision);
1104  lat_ss << point.y();
1105  lat_lon_text += lat_ss.str().c_str();
1106 
1107  lat_lon_text += ", ";
1108 
1109  double lon_scale = (1.0 / (111111.0 * std::cos(point.y() * swri_math_util::_deg_2_rad))) * scale;
1110  int32_t lon_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lon_scale))));
1111 
1112  std::ostringstream lon_ss;
1113  lon_ss << std::fixed << std::setprecision(lon_precision);
1114  lon_ss << point.x();
1115  lat_lon_text += lon_ss.str().c_str();
1116 
1117  lat_lon_pos_label_->setText(lat_lon_text);
1118  lat_lon_pos_label_->setVisible(true);
1119  lat_lon_pos_label_->update();
1120  }
1121  else if (lat_lon_pos_label_->isVisible())
1122  {
1123  lat_lon_pos_label_->setVisible(false);
1124  }
1125  }
1126 }
1127 
1129  const std::string& name,
1130  const std::string& type,
1131  bool visible,
1132  bool collapsed,
1133  int draw_order)
1134 {
1135  ConfigItem* config_item = new ConfigItem();
1136 
1137  config_item->SetName(name.c_str());
1138 
1139  std::string real_type = type;
1140  if (real_type == "mapviz_plugins/mutlires_image")
1141  {
1142  // The "multires_image" plugin was originally accidentally named "mutlires_image".
1143  // Loading a mapviz config file that still has the old name would normally cause it
1144  // to crash, so this will check for and correct it.
1145  real_type = "mapviz_plugins/multires_image";
1146  }
1147 
1148 
1149  ROS_INFO("creating: %s", real_type.c_str());
1150  MapvizPluginPtr plugin = loader_->createInstance(real_type.c_str());
1151 
1152  // Setup configure widget
1153  config_item->SetWidget(plugin->GetConfigWidget(this));
1154  plugin->SetIcon(config_item->ui_.icon);
1155  plugin->Initialize(tf_, tf_manager_, canvas_);
1156  plugin->SetType(real_type.c_str());
1157  plugin->SetName(name);
1158  plugin->SetNode(*node_);
1159  plugin->SetVisible(visible);
1160 
1161  if (draw_order == 0)
1162  {
1163  plugin->SetDrawOrder(ui_.configs->count());
1164  }
1165  else if (draw_order > 0)
1166  {
1167  plugin->SetDrawOrder(std::min(ui_.configs->count(), draw_order - 1));
1168  }
1169  else if (draw_order < 0)
1170  {
1171  plugin->SetDrawOrder(std::max(0, ui_.configs->count() + draw_order + 1));
1172  }
1173 
1174  QString pretty_type(real_type.c_str());
1175  pretty_type = pretty_type.split('/').last();
1176  config_item->SetType(pretty_type);
1177  QListWidgetItem* item = new PluginConfigListItem();
1178  config_item->SetListItem(item);
1179  item->setSizeHint(config_item->sizeHint());
1180  connect(config_item, SIGNAL(UpdateSizeHint()), this, SLOT(UpdateSizeHints()));
1181  connect(config_item, SIGNAL(ToggledDraw(QListWidgetItem*, bool)), this, SLOT(ToggleShowPlugin(QListWidgetItem*, bool)));
1182  connect(config_item, SIGNAL(RemoveRequest(QListWidgetItem*)), this, SLOT(RemoveDisplay(QListWidgetItem*)));
1183  connect(plugin.get(), SIGNAL(VisibleChanged(bool)), config_item, SLOT(ToggleDraw(bool)));
1184  connect(plugin.get(), SIGNAL(SizeChanged()), this, SLOT(UpdateSizeHints()));
1185 
1186  if (real_type == "mapviz_plugins/image")
1187  {
1188  // This is a little kludgey because we're relying on hard-coding a
1189  // plugin type here... feel free to suggest a better way.
1190  // If the default image transport has changed, we want to notify all of our
1191  // image plugins of it so that they will resubscribe appropriately.
1192  connect(this, SIGNAL(ImageTransportChanged()),
1193  plugin.get(), SLOT(Resubscribe()));
1194  }
1195 
1196  if (draw_order == 0)
1197  {
1198  ui_.configs->addItem(item);
1199  }
1200  else
1201  {
1202  ui_.configs->insertItem(plugin->DrawOrder(), item);
1203  }
1204 
1205  ui_.configs->setItemWidget(item, config_item);
1206  ui_.configs->UpdateIndices();
1207 
1208  // Add plugin to canvas
1209  plugin->SetTargetFrame(ui_.fixedframe->currentText().toStdString());
1210  plugins_[item] = plugin;
1211  canvas_->AddPlugin(plugin, -1);
1212 
1213  config_item->ToggleDraw(visible);
1214 
1215  if (collapsed)
1216  config_item->Hide();
1217 
1218  ReorderDisplays();
1219 
1220  return plugin;
1221 }
1222 
1223 void Mapviz::ToggleShowPlugin(QListWidgetItem* item, bool visible)
1224 {
1225  ROS_INFO("Toggle show plugin");
1226 
1227  if (plugins_.count(item) == 1)
1228  {
1229  plugins_[item]->SetVisible(visible);
1230  }
1231  canvas_->UpdateView();
1232 }
1233 
1234 void Mapviz::FixedFrameSelected(const QString& text)
1235 {
1236  if (!updating_frames_)
1237  {
1238  ROS_INFO("Fixed frame selected: %s", text.toStdString().c_str());
1239  if (canvas_ != NULL)
1240  {
1241  canvas_->SetFixedFrame(text.toStdString().c_str());
1242  }
1243  }
1244 }
1245 
1246 void Mapviz::TargetFrameSelected(const QString& text)
1247 {
1248  if (!updating_frames_)
1249  {
1250  ROS_INFO("Target frame selected: %s", text.toStdString().c_str());
1251 
1252  if (canvas_ != NULL)
1253  {
1254  canvas_->SetTargetFrame(text.toStdString().c_str());
1255  }
1256  }
1257 }
1258 
1260 {
1262 }
1263 
1265 {
1266  canvas_->ToggleRotate90(on);
1267 }
1268 
1270 {
1272 }
1273 
1275 {
1276  if (on)
1277  {
1278  ui_.configdock->show();
1279  }
1280  else
1281  {
1282  ui_.configdock->hide();
1283  }
1284 
1285  AdjustWindowSize();
1286 }
1287 
1289 {
1290  ui_.statusbar->setVisible(on);
1291 
1292  AdjustWindowSize();
1293 }
1294 
1296 {
1297  if (on)
1298  {
1299  ui_.actionShow_Status_Bar->setChecked(true);
1300  }
1301 
1302  screenshot_button_->setVisible(on);
1303  rec_button_->setVisible(on);
1304  stop_button_->setVisible(on);
1305  spacer1_->setVisible(on);
1306  spacer2_->setVisible(on);
1307  spacer3_->setVisible(on);
1308 }
1309 
1311 {
1312  stop_button_->setEnabled(true);
1313 
1314  if (on)
1315  {
1316  rec_button_->setIcon(QIcon(":/images/media-playback-pause.png"));
1317  rec_button_->setToolTip("Pause recording video of display canvas");
1318  if (!vid_writer_->isRecording())
1319  {
1320  // Lock the window size.
1321  AdjustWindowSize();
1322 
1323  canvas_->CaptureFrames(true);
1324 
1325  std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
1326  boost::replace_all(posix_time, ".", "_");
1327  std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".avi";
1328  boost::replace_all(filename, "~", getenv("HOME"));
1329 
1330 
1331  if (!vid_writer_->initializeWriter(filename, canvas_->width(), canvas_->height()))
1332  {
1333  ROS_ERROR("Failed to open video file for writing.");
1334  StopRecord();
1335  return;
1336  }
1337 
1338  ROS_INFO("Writing video to: %s", filename.c_str());
1339  ui_.statusbar->showMessage("Recording video to " + QString::fromStdString(filename));
1340 
1341  canvas_->updateGL();
1342  }
1343 
1344  record_timer_.start(1000.0 / 30.0);
1345  }
1346  else
1347  {
1348  rec_button_->setIcon(QIcon(":/images/media-record.png"));
1349  rec_button_->setToolTip("Continue recording video of display canvas");
1350  record_timer_.stop();
1351  }
1352 }
1353 
1354 void Mapviz::SetImageTransport(QAction* transport_action)
1355 {
1356  std::string transport = transport_action->text().toStdString();
1357  ROS_INFO("Setting %s to %s", IMAGE_TRANSPORT_PARAM.c_str(), transport.c_str());
1358  node_->setParam(IMAGE_TRANSPORT_PARAM, transport);
1359 
1360  Q_EMIT(ImageTransportChanged());
1361 }
1362 
1364 {
1365  QList<QAction*> actions = image_transport_menu_->actions();
1366 
1367  std::string current_transport;
1368  node_->param<std::string>(IMAGE_TRANSPORT_PARAM, current_transport, "raw");
1369  Q_FOREACH(QAction* action, actions)
1370  {
1371  if (action->text() == QString::fromStdString(current_transport))
1372  {
1373  action->setChecked(true);
1374  return;
1375  }
1376  }
1377 
1378  ROS_WARN("%s param was set to an unrecognized value: %s",
1379  IMAGE_TRANSPORT_PARAM.c_str(), current_transport.c_str());
1380 }
1381 
1383 {
1384  // We need to store the data inside a QImage in order to emit it as a
1385  // signal.
1386  // Note that the QImage here is set to "ARGB32", but it is actually BGRA.
1387  // Qt doesn't have a comparable BGR format, and the cv::VideoWriter this
1388  // is going to expects BGR format, but it'd be a waste for us to convert
1389  // to RGB and then back to BGR.
1390  QImage frame(canvas_->width(), canvas_->height(), QImage::Format_ARGB32);
1391  if (canvas_->CopyCaptureBuffer(frame.bits()))
1392  {
1393  Q_EMIT(FrameGrabbed(frame));
1394  }
1395  else
1396  {
1397  ROS_ERROR("Failed to get capture buffer");
1398  }
1399 }
1400 
1402 {
1404 }
1405 
1407 {
1408  rec_button_->setChecked(false);
1409  stop_button_->setEnabled(false);
1410 
1411  record_timer_.stop();
1412  if (vid_writer_)
1413  {
1414  vid_writer_->stop();
1415  }
1416  canvas_->CaptureFrames(false);
1417 
1418  ui_.statusbar->showMessage(QString(""));
1419  rec_button_->setToolTip("Start recording video of display canvas");
1420 
1421  AdjustWindowSize();
1422 }
1423 
1425 {
1426  canvas_->CaptureFrame(true);
1427 
1428  std::vector<uint8_t> frame;
1429  if (canvas_->CopyCaptureBuffer(frame))
1430  {
1431  cv::Mat image(canvas_->height(), canvas_->width(), CV_8UC4, &frame[0]);
1432  cv::Mat screenshot;
1433  cvtColor(image, screenshot, cv::COLOR_BGRA2BGR);
1434 
1435  cv::flip(screenshot, screenshot, 0);
1436 
1437  std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
1438  boost::replace_all(posix_time, ".", "_");
1439  std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".png";
1440  boost::replace_all(filename, "~", getenv("HOME"));
1441 
1442  ROS_INFO("Writing screenshot to: %s", filename.c_str());
1443  ui_.statusbar->showMessage("Saved image to " + QString::fromStdString(filename));
1444 
1445  cv::imwrite(filename, screenshot);
1446  }
1447  else
1448  {
1449  ROS_ERROR("Failed to take screenshot.");
1450  }
1451 }
1452 
1454 {
1455  for (int i = 0; i < ui_.configs->count(); i++)
1456  {
1457  QListWidgetItem* item = ui_.configs->item(i);
1458  ConfigItem* widget = static_cast<ConfigItem*>(ui_.configs->itemWidget(item));
1459  if (widget) {
1460  // Make sure the ConfigItem in the QListWidgetItem we're getting really
1461  // exists; if this method is called before it's been initialized, it would
1462  // cause a crash.
1463  item->setSizeHint(widget->sizeHint());
1464  }
1465  }
1466 }
1467 
1469 {
1470  QListWidgetItem* item = ui_.configs->takeItem(ui_.configs->currentRow());
1471  RemoveDisplay(item);
1472 }
1473 
1474 void Mapviz::RemoveDisplay(QListWidgetItem* item)
1475 {
1476  ROS_INFO("Remove display ...");
1477 
1478  if (item)
1479  {
1480  canvas_->RemovePlugin(plugins_[item]);
1481  plugins_.erase(item);
1482 
1483  delete item;
1484  }
1485 }
1486 
1488 {
1489  while (ui_.configs->count() > 0)
1490  {
1491  ROS_INFO("Remove display ...");
1492 
1493  QListWidgetItem* item = ui_.configs->takeItem(0);
1494 
1495  canvas_->RemovePlugin(plugins_[item]);
1496  plugins_.erase(item);
1497 
1498  delete item;
1499  }
1500 }
1501 
1503 {
1504  ROS_INFO("Reorder displays");
1505  for (int i = 0; i < ui_.configs->count(); i++)
1506  {
1507  plugins_[ui_.configs->item(i)]->SetDrawOrder(i);
1508  }
1510 }
1511 
1512 void Mapviz::SelectBackgroundColor(const QColor &color)
1513 {
1514  background_ = color;
1516 }
1517 
1519 {
1520  QFileDialog dialog(this, "Select Capture Directory");
1521  dialog.setFileMode(QFileDialog::DirectoryOnly);
1522 
1523  dialog.exec();
1524 
1525  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
1526  {
1527  capture_directory_ = dialog.selectedFiles().first().toStdString();
1528  }
1529 }
1530 
1532 {
1533  ROS_INFO("Mapviz Profiling Data");
1534  meas_spin_.printInfo("ROS SpinOnce()");
1535  for (auto& display: plugins_)
1536  {
1537  MapvizPluginPtr plugin = display.second;
1538  if (plugin)
1539  {
1540  plugin->PrintMeasurements();
1541  }
1542  }
1543 }
1544 }
mapviz::Stopwatch::start
void start()
Definition: stopwatch.h:51
ros::init_options::AnonymousName
AnonymousName
mapviz::Mapviz::ClearHistory
void ClearHistory()
Definition: mapviz.cpp:932
mapviz::MapCanvas::UpdateView
void UpdateView()
Definition: map_canvas.cpp:560
mapviz::MapCanvas::CopyCaptureBuffer
bool CopyCaptureBuffer(uchar *buffer)
Definition: map_canvas.h:131
mapviz::Mapviz::argc_
int argc_
Definition: mapviz.h:156
mapviz::Mapviz::video_thread_
QThread video_thread_
Definition: mapviz.h:169
mapviz::Mapviz::TargetFrameSelected
void TargetFrameSelected(const QString &text)
Definition: mapviz.cpp:1246
mapviz::Mapviz::rec_button_
QPushButton * rec_button_
Definition: mapviz.h:152
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
mapviz::ConfigItem::SetName
void SetName(QString name)
Definition: config_item.cpp:78
config_item.h
mapviz::MapCanvas::ToggleRotate90
void ToggleRotate90(bool on)
Definition: map_canvas.cpp:436
mapviz
Definition: color_button.h:36
mapviz::Mapviz::Force720p
void Force720p(bool on)
Definition: mapviz.cpp:431
mapviz.h
mapviz::Mapviz::lat_lon_pos_label_
QLabel * lat_lon_pos_label_
Definition: mapviz.h:146
NULL
#define NULL
mapviz::Mapviz::ToggleCaptureTools
void ToggleCaptureTools(bool on)
Definition: mapviz.cpp:1295
image_transport::ImageTransport
swri_yaml_util::FindValue
bool FindValue(const YAML::Node &node, const std::string &name)
mapviz::Mapviz::Recenter
void Recenter()
Definition: mapviz.cpp:1401
mapviz::MapCanvas::SetBackground
void SetBackground(const QColor &color)
Definition: map_canvas.h:112
boost::shared_ptr
swri_yaml_util::LoadFile
bool LoadFile(const std::string &path, YAML::Node &yaml)
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
mapviz::Mapviz::AddDisplay
bool AddDisplay(AddMapvizDisplay::Request &req, AddMapvizDisplay::Response &resp)
Definition: mapviz.cpp:980
mapviz::Mapviz::plugins_
std::map< QListWidgetItem *, MapvizPluginPtr > plugins_
Definition: mapviz.h:181
mapviz::Mapviz::capture_directory_
std::string capture_directory_
Definition: mapviz.h:168
mapviz::Mapviz::ui_
Ui::mapviz ui_
Definition: mapviz.h:135
mapviz::MapCanvas::CaptureFrame
void CaptureFrame(bool force=false)
Definition: map_canvas.cpp:183
mapviz::VideoWriter::stop
void stop()
Definition: video_writer.cpp:115
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
mapviz::MapCanvas::OffsetY
float OffsetY() const
Definition: map_canvas.h:84
mapviz::MapCanvas::RemovePlugin
void RemovePlugin(MapvizPluginPtr plugin)
Definition: map_canvas.cpp:456
mapviz::Mapviz::SetCaptureDirectory
void SetCaptureDirectory()
Definition: mapviz.cpp:1518
mapviz::Mapviz::frame_timer_
QTimer frame_timer_
Definition: mapviz.h:139
mapviz::Mapviz::MAPVIZ_CONFIG_FILE
static const QString MAPVIZ_CONFIG_FILE
Definition: mapviz.h:206
mapviz::VideoWriter::isRecording
bool isRecording()
Definition: video_writer.cpp:64
ros::spinOnce
ROSCPP_DECL void spinOnce()
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
mapviz::ConfigItem::SetType
void SetType(QString type)
Definition: config_item.cpp:84
mapviz::Mapviz::image_transport_menu_
QMenu * image_transport_menu_
Definition: mapviz.h:137
mapviz::Mapviz::force_720p_
bool force_720p_
Definition: mapviz.h:161
mapviz::MapCanvas::InitializeTf
void InitializeTf(boost::shared_ptr< tf::TransformListener > tf)
Definition: map_canvas.cpp:103
mapviz::Mapviz::ClearConfig
void ClearConfig()
Definition: mapviz.cpp:883
mapviz::Stopwatch::stop
void stop()
Definition: stopwatch.h:59
mapviz::Mapviz::spacer1_
QWidget * spacer1_
Definition: mapviz.h:148
mapviz::MapCanvas::OffsetX
float OffsetX() const
Definition: map_canvas.h:83
mapviz::Mapviz::profile_timer_
QTimer profile_timer_
Definition: mapviz.h:143
mapviz::Mapviz::Initialize
void Initialize()
Definition: mapviz.cpp:231
mapviz::Mapviz::SelectNewDisplay
void SelectNewDisplay()
Definition: mapviz.cpp:941
mapviz::Mapviz::UpdateImageTransportMenu
void UpdateImageTransportMenu()
Definition: mapviz.cpp:1363
ros::ok
ROSCPP_DECL bool ok()
mapviz::Mapviz::xy_pos_label_
QLabel * xy_pos_label_
Definition: mapviz.h:145
mapviz::Mapviz::Mapviz
Mapviz(bool is_standalone, int argc, char **argv, QWidget *parent=0, Qt::WindowFlags flags=0)
Definition: mapviz.cpp:87
mapviz::Mapviz::tf_manager_
swri_transform_util::TransformManagerPtr tf_manager_
Definition: mapviz.h:177
mapviz::Mapviz::node_
ros::NodeHandle * node_
Definition: mapviz.h:174
frames.h
mapviz::Mapviz::background_
QColor background_
Definition: mapviz.h:164
mapviz::Mapviz::ToggleStatusBar
void ToggleStatusBar(bool on)
Definition: mapviz.cpp:1288
mapviz::Mapviz::ReorderDisplays
void ReorderDisplays()
Definition: mapviz.cpp:1502
mapviz::ConfigItem::Hide
void Hide()
Definition: config_item.cpp:117
mapviz::Mapviz::spin_timer_
QTimer spin_timer_
Definition: mapviz.h:140
mapviz::Mapviz::is_standalone_
bool is_standalone_
Definition: mapviz.h:159
mapviz::ConfigItem::SetWidget
void SetWidget(QWidget *widget)
Definition: config_item.cpp:90
mapviz::MapCanvas::SetOffsetX
void SetOffsetX(float x)
Definition: map_canvas.h:100
yaml_util.h
mapviz::Mapviz::SaveConfigAs
void SaveConfigAs()
Definition: mapviz.cpp:899
mapviz::Mapviz::ToggleShowPlugin
void ToggleShowPlugin(QListWidgetItem *item, bool visible)
Definition: mapviz.cpp:1223
mapviz::Mapviz::SetImageTransport
void SetImageTransport(QAction *transport_action)
Definition: mapviz.cpp:1354
mapviz::Mapviz::record_timer_
QTimer record_timer_
Definition: mapviz.h:142
mapviz::Mapviz::force_480p_
bool force_480p_
Definition: mapviz.h:162
mapviz::Mapviz::ToggleRotate90
void ToggleRotate90(bool on)
Definition: mapviz.cpp:1264
mapviz::Mapviz::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: mapviz.cpp:218
mapviz::Mapviz::RemoveDisplay
void RemoveDisplay()
Definition: mapviz.cpp:1468
mapviz::Mapviz::FixedFrameSelected
void FixedFrameSelected(const QString &text)
Definition: mapviz.cpp:1234
mapviz::Mapviz::SpinOnce
void SpinOnce()
Definition: mapviz.cpp:339
mapviz::Mapviz::ToggleEnableAntialiasing
void ToggleEnableAntialiasing(bool on)
Definition: mapviz.cpp:1269
ROS_DEBUG
#define ROS_DEBUG(...)
mapviz::Mapviz::ImageTransportChanged
void ImageTransportChanged()
mapviz::Mapviz::CaptureVideoFrame
void CaptureVideoFrame()
Definition: mapviz.cpp:1382
ros::WallTime::now
static WallTime now()
mapviz::ConfigItem::ToggleDraw
void ToggleDraw(bool toggled)
Definition: config_item.cpp:56
mapviz::MapCanvas::SetFixedFrame
void SetFixedFrame(const std::string &frame)
Definition: map_canvas.cpp:411
mapviz::Mapviz::Hover
void Hover(double x, double y, double scale)
Definition: mapviz.cpp:1049
mapviz::Mapviz::ToggleConfigPanel
void ToggleConfigPanel(bool on)
Definition: mapviz.cpp:1274
mapviz::MapCanvas::ReorderDisplays
void ReorderDisplays()
Definition: map_canvas.cpp:582
mapviz::Mapviz::save_timer_
QTimer save_timer_
Definition: mapviz.h:141
mapviz::Mapviz::showEvent
virtual void showEvent(QShowEvent *event)
Definition: mapviz.cpp:213
constants.h
swri_transform_util::Transform
mapviz::Mapviz::screenshot_button_
QPushButton * screenshot_button_
Definition: mapviz.h:154
mapviz::Mapviz::FrameGrabbed
void FrameGrabbed(QImage)
mapviz::Mapviz::loader_
pluginlib::ClassLoader< MapvizPlugin > * loader_
Definition: mapviz.h:179
ROS_WARN
#define ROS_WARN(...)
mapviz::Mapviz::canvas_
MapCanvas * canvas_
Definition: mapviz.h:180
mapviz::Mapviz::meas_spin_
Stopwatch meas_spin_
Definition: mapviz.h:183
mapviz::VideoWriter
Definition: video_writer.h:45
mapviz::Mapviz::UpdateFrames
void UpdateFrames()
Definition: mapviz.cpp:353
mapviz::Mapviz::HandleProfileTimer
void HandleProfileTimer()
Definition: mapviz.cpp:1531
mapviz::Mapviz::ROS_WORKSPACE_VAR
static const QString ROS_WORKSPACE_VAR
Definition: mapviz.h:205
mapviz::Mapviz::AutoSave
void AutoSave()
Definition: mapviz.cpp:832
pluginlib::ClassLoader
mapviz::Mapviz::initialized_
bool initialized_
Definition: mapviz.h:160
mapviz::ConfigItem
Definition: config_item.h:48
mapviz::Mapviz::ClearDisplays
void ClearDisplays()
Definition: mapviz.cpp:1487
mapviz::Mapviz::resizable_
bool resizable_
Definition: mapviz.h:163
mapviz::PluginConfigListItem
Definition: widgets.h:75
mapviz::Mapviz::save_location_
std::string save_location_
Definition: mapviz.h:166
mapviz::MapCanvas::ResetLocation
void ResetLocation()
Definition: map_canvas.cpp:576
image_transport.h
mapviz::Mapviz::UpdateSizeHints
void UpdateSizeHints()
Definition: mapviz.cpp:1453
mapviz::Mapviz::ToggleFixOrientation
void ToggleFixOrientation(bool on)
Definition: mapviz.cpp:1259
mapviz::Mapviz::Force480p
void Force480p(bool on)
Definition: mapviz.cpp:447
mapviz::MapCanvas::SetOffsetY
void SetOffsetY(float y)
Definition: map_canvas.h:106
mapviz::MapCanvas
Definition: map_canvas.h:57
swri_transform_util::_wgs84_frame
static const std::string _wgs84_frame
mapviz::Mapviz::Save
void Save(const std::string &filename)
Definition: mapviz.cpp:756
mapviz::Mapviz::OpenConfig
void OpenConfig()
Definition: mapviz.cpp:866
mapviz::Mapviz::vid_writer_
VideoWriter * vid_writer_
Definition: mapviz.h:170
mapviz::Mapviz::StopRecord
void StopRecord()
Definition: mapviz.cpp:1406
mapviz::Mapviz::ToggleRecord
void ToggleRecord(bool on)
Definition: mapviz.cpp:1310
mapviz::VideoWriter::initializeWriter
bool initializeWriter(const std::string &directory, int width, int height)
Definition: video_writer.cpp:38
mapviz::MapCanvas::ToggleFixOrientation
void ToggleFixOrientation(bool on)
Definition: map_canvas.cpp:431
mapviz::Mapviz::stop_button_
QPushButton * stop_button_
Definition: mapviz.h:153
mapviz::ConfigItem::SetListItem
void SetListItem(QListWidgetItem *item)
Definition: config_item.h:60
swri_math_util::_deg_2_rad
static const long double _deg_2_rad
mapviz::MapCanvas::SetViewScale
void SetViewScale(float scale)
Definition: map_canvas.h:94
mapviz::Mapviz::AdjustWindowSize
void AdjustWindowSize()
Definition: mapviz.cpp:479
ROS_ERROR
#define ROS_ERROR(...)
mapviz::Mapviz::CreateNewDisplay
MapvizPluginPtr CreateNewDisplay(const std::string &name, const std::string &type, bool visible, bool collapsed, int draw_order=0)
Definition: mapviz.cpp:1128
mapviz::MapCanvas::ViewScale
float ViewScale() const
Definition: map_canvas.h:82
image_transport
mapviz::MapCanvas::CaptureFrames
void CaptureFrames(bool enabled)
Definition: map_canvas.h:118
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
swri_yaml_util::LoadMap
bool LoadMap(const std::map< std::string, std::string > &dict, YAML::Node &yaml)
pluginlib::LibraryLoadException
mapviz::Mapviz::Screenshot
void Screenshot()
Definition: mapviz.cpp:1424
mapviz::Mapviz::spacer3_
QWidget * spacer3_
Definition: mapviz.h:150
mapviz::Mapviz::argv_
char ** argv_
Definition: mapviz.h:157
mapviz::ConfigItem::ui_
Ui::configitem ui_
Definition: config_item.h:64
image_transport::ImageTransport::getLoadableTransports
std::vector< std::string > getLoadableTransports() const
mapviz::MapCanvas::ToggleEnableAntialiasing
void ToggleEnableAntialiasing(bool on)
Definition: map_canvas.cpp:441
mapviz::Mapviz::add_display_srv_
ros::ServiceServer add_display_srv_
Definition: mapviz.h:175
mapviz::Mapviz::SelectBackgroundColor
void SelectBackgroundColor(const QColor &color)
Definition: mapviz.cpp:1512
ROS_INFO
#define ROS_INFO(...)
mapviz::Mapviz::SaveConfig
void SaveConfig()
Definition: mapviz.cpp:888
mapviz::Mapviz::Open
void Open(const std::string &filename)
Definition: mapviz.cpp:524
config
config
mapviz::MapCanvas::SetTargetFrame
void SetTargetFrame(const std::string &frame)
Definition: map_canvas.cpp:421
mapviz::Mapviz::tf_
boost::shared_ptr< tf::TransformListener > tf_
Definition: mapviz.h:176
mapviz::Mapviz::spacer2_
QWidget * spacer2_
Definition: mapviz.h:149
mapviz::Mapviz::updating_frames_
bool updating_frames_
Definition: mapviz.h:172
mapviz::Mapviz::SetResizable
void SetResizable(bool on)
Definition: mapviz.cpp:463
mapviz::MapCanvas::AddPlugin
void AddPlugin(MapvizPluginPtr plugin, int order)
Definition: map_canvas.cpp:451
mapviz::Mapviz::IMAGE_TRANSPORT_PARAM
static const std::string IMAGE_TRANSPORT_PARAM
Definition: mapviz.h:207
name
string name
mapviz::Stopwatch::printInfo
void printInfo(const std::string &name) const
Definition: stopwatch.h:87
ros::NodeHandle
mapviz::Mapviz::recenter_button_
QPushButton * recenter_button_
Definition: mapviz.h:151
mapviz::Mapviz::~Mapviz
~Mapviz()
Definition: mapviz.cpp:206
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)


mapviz
Author(s): Marc Alban
autogenerated on Wed Jan 17 2024 03:27:46