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;
688  doc[IMAGE_TRANSPORT_PARAM] >> image_transport;
689 
690  node_->setParam(IMAGE_TRANSPORT_PARAM, image_transport);
691  }
692 
693  bool use_latest_transforms = true;
694  if (swri_yaml_util::FindValue(doc, "use_latest_transforms"))
695  {
696  doc["use_latest_transforms"] >> use_latest_transforms;
697  }
698  ui_.uselatesttransforms->setChecked(use_latest_transforms);
699  canvas_->ToggleUseLatestTransforms(use_latest_transforms);
700 
701  if (swri_yaml_util::FindValue(doc, "background"))
702  {
703  std::string color;
704  doc["background"] >> color;
705  background_ = QColor(color.c_str());
706  ui_.bg_color->setColor(background_);
708  }
709 
710  if (swri_yaml_util::FindValue(doc, "displays"))
711  {
712  const YAML::Node& displays = doc["displays"];
713  for (uint32_t i = 0; i < displays.size(); i++)
714  {
715  std::string type, name;
716  displays[i]["type"] >> type;
717  displays[i]["name"] >> name;
718 
719  const YAML::Node& config = displays[i]["config"];
720 
721  bool visible = false;
722  config["visible"] >> visible;
723 
724  bool collapsed = false;
725  config["collapsed"] >> collapsed;
726 
727  try
728  {
729  MapvizPluginPtr plugin =
730  CreateNewDisplay(name, type, visible, collapsed);
731  plugin->LoadConfig(config, config_path);
732  plugin->DrawIcon();
733  }
734  catch (const pluginlib::LibraryLoadException& e)
735  {
736  failed_plugins.push_back(type);
737  ROS_ERROR("%s", e.what());
738  }
739  }
740  }
741  }
742  catch (const YAML::ParserException& e)
743  {
744  ROS_ERROR("%s", e.what());
745  return;
746  }
747  catch (const YAML::Exception& e)
748  {
749  ROS_ERROR("%s", e.what());
750  return;
751  }
752 
753  if (!failed_plugins.empty())
754  {
755  std::stringstream message;
756  message << "The following plugin(s) failed to load:" << std::endl;
757  std::string failures = boost::algorithm::join(failed_plugins, "\n");
758  message << failures << std::endl << std::endl << "Check the ROS log for more details.";
759 
760  QMessageBox::warning(this, "Failed to load plugins", QString::fromStdString(message.str()));
761  }
762 }
763 
764 void Mapviz::Save(const std::string& filename)
765 {
766  std::ofstream fout(filename.c_str());
767  if (fout.fail())
768  {
769  ROS_ERROR("Failed to open file: %s", filename.c_str());
770  return;
771  }
772 
773  boost::filesystem::path filepath(filename);
774  std::string config_path = filepath.parent_path().string();
775 
776  YAML::Emitter out;
777 
778  out << YAML::BeginMap;
779  out << YAML::Key << "capture_directory" << YAML::Value << capture_directory_;
780  out << YAML::Key << "fixed_frame" << YAML::Value << ui_.fixedframe->currentText().toStdString();
781  out << YAML::Key << "target_frame" << YAML::Value << ui_.targetframe->currentText().toStdString();
782  out << YAML::Key << "fix_orientation" << YAML::Value << ui_.actionFix_Orientation->isChecked();
783  out << YAML::Key << "rotate_90" << YAML::Value << ui_.actionRotate_90->isChecked();
784  out << YAML::Key << "enable_antialiasing" << YAML::Value << ui_.actionEnable_Antialiasing->isChecked();
785  out << YAML::Key << "show_displays" << YAML::Value << ui_.actionConfig_Dock->isChecked();
786  out << YAML::Key << "show_status_bar" << YAML::Value << ui_.actionShow_Status_Bar->isChecked();
787  out << YAML::Key << "show_capture_tools" << YAML::Value << ui_.actionShow_Capture_Tools->isChecked();
788  out << YAML::Key << "window_width" << YAML::Value << width();
789  out << YAML::Key << "window_height" << YAML::Value << height();
790  out << YAML::Key << "view_scale" << YAML::Value << canvas_->ViewScale();
791  out << YAML::Key << "offset_x" << YAML::Value << canvas_->OffsetX();
792  out << YAML::Key << "offset_y" << YAML::Value << canvas_->OffsetY();
793  out << YAML::Key << "use_latest_transforms" << YAML::Value << ui_.uselatesttransforms->isChecked();
794  out << YAML::Key << "background" << YAML::Value << background_.name().toStdString();
795  std::string image_transport;
796  if (node_->getParam(IMAGE_TRANSPORT_PARAM, image_transport))
797  {
798  out << YAML::Key << IMAGE_TRANSPORT_PARAM << YAML::Value << image_transport;
799  }
800 
801  if (force_720p_)
802  {
803  out << YAML::Key << "force_720p" << YAML::Value << force_720p_;
804  }
805 
806  if (force_480p_)
807  {
808  out << YAML::Key << "force_480p" << YAML::Value << force_480p_;
809  }
810 
811  if (ui_.configs->count() > 0)
812  {
813  out << YAML::Key << "displays"<< YAML::Value << YAML::BeginSeq;
814 
815  for (int i = 0; i < ui_.configs->count(); i++)
816  {
817  out << YAML::BeginMap;
818  out << YAML::Key << "type" << YAML::Value << plugins_[ui_.configs->item(i)]->Type();
819  out << YAML::Key << "name" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Name().toStdString();
820  out << YAML::Key << "config" << YAML::Value;
821  out << YAML::BeginMap;
822 
823  out << YAML::Key << "visible" << YAML::Value << plugins_[ui_.configs->item(i)]->Visible();
824  out << YAML::Key << "collapsed" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Collapsed();
825 
826  plugins_[ui_.configs->item(i)]->SaveConfig(out, config_path);
827 
828  out << YAML::EndMap;
829  out << YAML::EndMap;
830  }
831 
832  out << YAML::EndSeq;
833  }
834 
835  out << YAML::EndMap;
836 
837  fout << out.c_str();
838  fout.close();
839 }
840 
842 {
843  QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
844  QString default_path = QDir::homePath();
845 
846  if (env.contains(ROS_WORKSPACE_VAR))
847  {
848  // Try to save our config in the ROS_WORKSPACE directory, but if we can't write
849  // to that -- probably because it is read-only -- try to use the home directory
850  // instead.
851  QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
852  QString ws_file = ws_path + MAPVIZ_CONFIG_FILE;
853  QFileInfo file_info(ws_file);
854  QFileInfo dir_info(ws_path);
855  if ((!file_info.exists() && dir_info.isWritable()) ||
856  file_info.isWritable())
857  {
858  // Note that FileInfo::isWritable will return false if a file does not exist, so
859  // we need to check both if the target file is writable and if the target dir is
860  // writable if the file doesn't exist.
861  default_path = ws_path;
862  }
863  else
864  {
865  ROS_WARN("Could not write config file to %s. Trying home directory.",
866  (ws_path + MAPVIZ_CONFIG_FILE).toStdString().c_str());
867  }
868  }
869  default_path += MAPVIZ_CONFIG_FILE;
870 
871 
872  Save(default_path.toStdString());
873 }
874 
876 {
877  QFileDialog dialog(this, "Select Config File");
878  dialog.setFileMode(QFileDialog::ExistingFile);
879  dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
880  std::string directory = save_location_.substr(0, save_location_.find_last_of('/') + 1);
881  dialog.setDirectory(QString(directory.c_str()));
882 
883  dialog.exec();
884 
885  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
886  {
887  std::string path = dialog.selectedFiles().first().toStdString();
888  Open(path);
889  }
890 }
891 
893 {
894  ClearDisplays();
895 }
896 
898 {
899  if (save_location_.length() == 0)
900  {
901  SaveConfigAs();
902  return;
903  }
904 
906 }
907 
909 {
910  QFileDialog dialog(this, "Save Config File");
911  dialog.setFileMode(QFileDialog::AnyFile);
912  dialog.setAcceptMode(QFileDialog::AcceptSave);
913  dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
914  dialog.setDefaultSuffix("mvc");
915  std::string directory = save_location_.substr(0, save_location_.find_last_of('/') + 1);
916  dialog.setDirectory(QString(directory.c_str()));
917  dialog.exec();
918 
919  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
920  {
921  std::string path = dialog.selectedFiles().first().toStdString();
922 
923  std::string title;
924  size_t last_slash = path.find_last_of('/');
925  if (last_slash != std::string::npos && last_slash != path.size() - 1)
926  {
927  title = path.substr(last_slash + 1) + " (" +
928  path.substr(0, last_slash + 1) + ")";
929  }
930  else
931  {
932  title = path;
933  }
934  title += " - mapviz";
935  setWindowTitle(QString::fromStdString(title));
936  Save(path);
937  save_location_ = path;
938  }
939 }
940 
942 {
943  ROS_DEBUG("Mapviz::ClearHistory()");
944  for (auto& plugin: plugins_)
945  {
946  plugin.second->ClearHistory();
947  }
948 }
949 
951 {
952  ROS_INFO("Select new display ...");
953  QDialog dialog;
954  Ui::pluginselect ui;
955  ui.setupUi(&dialog);
956 
957  std::vector<std::string> plugins = loader_->getDeclaredClasses();
958  std::map<std::string, std::string> plugin_types;
959  for (size_t i = 0; i < plugins.size(); i++)
960  {
961  QString type(plugins[i].c_str());
962  type = type.split('/').last();
963  ui.displaylist->addItem(type);
964  plugin_types[type.toStdString()] = plugins[i];
965  }
966  ui.displaylist->setCurrentRow(0);
967 
968  dialog.exec();
969 
970  if (dialog.result() == QDialog::Accepted)
971  {
972  std::string type_name = ui.displaylist->selectedItems().first()->text().toStdString();
973  std::string type = plugin_types[type_name];
974  std::string name = "new display";
975  try
976  {
977  CreateNewDisplay(name, type, true, false);
978  }
979  catch (const pluginlib::LibraryLoadException& e)
980  {
981  std::stringstream message;
982  message << "Unable to load " << type << "." << std::endl << "Check the ROS log for more details.";
983  QMessageBox::warning(this, "Plugin failed to load", QString::fromStdString(message.str()));
984  ROS_ERROR("%s", e.what());
985  }
986  }
987 }
988 
990  AddMapvizDisplay::Request& req,
991  AddMapvizDisplay::Response& resp)
992 {
993  std::map<std::string, std::string> properties;
994  for (auto& property: req.properties)
995  {
996  properties[property.key] = property.value;
997  }
998 
999  YAML::Node config;
1000  if (!swri_yaml_util::LoadMap(properties, config))
1001  {
1002  ROS_ERROR("Failed to parse properties into YAML.");
1003  return false;
1004  }
1005 
1006  for (auto& display: plugins_)
1007  {
1008  MapvizPluginPtr plugin = display.second;
1009  if (!plugin)
1010  {
1011  ROS_ERROR("Invalid plugin ptr.");
1012  continue;
1013  }
1014  if (plugin->Name() == req.name && plugin->Type() ==req.type)
1015  {
1016  plugin->LoadConfig(config, "");
1017  plugin->SetVisible(req.visible);
1018 
1019  if (req.draw_order > 0)
1020  {
1021  display.first->setData(Qt::UserRole, QVariant(req.draw_order - 1.1));
1022  ui_.configs->sortItems();
1023 
1024  ReorderDisplays();
1025  }
1026  else if (req.draw_order < 0)
1027  {
1028  display.first->setData(Qt::UserRole, QVariant(ui_.configs->count() + req.draw_order + 0.1));
1029  ui_.configs->sortItems();
1030 
1031  ReorderDisplays();
1032  }
1033 
1034  resp.success = true;
1035 
1036  return true;
1037  }
1038  }
1039 
1040  try
1041  {
1042  MapvizPluginPtr plugin =
1043  CreateNewDisplay(req.name, req.type, req.visible, false, req.draw_order);
1044  plugin->LoadConfig(config, "");
1045  plugin->DrawIcon();
1046  resp.success = true;
1047  }
1048  catch (const pluginlib::LibraryLoadException& e)
1049  {
1050  ROS_ERROR("%s", e.what());
1051  resp.success = false;
1052  resp.message = "Failed to load display plug-in.";
1053  }
1054 
1055  return true;
1056 }
1057 
1058 void Mapviz::Hover(double x, double y, double scale)
1059 {
1060  if (ui_.statusbar->isVisible())
1061  {
1062  if (scale == 0)
1063  {
1064  xy_pos_label_->setVisible(false);
1065  lat_lon_pos_label_->setVisible(false);
1066  return;
1067  }
1068 
1069  int32_t precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / scale))));
1070 
1071  QString text = ui_.fixedframe->currentText();
1072  if (text.isEmpty() || text == "/")
1073  {
1074  text = "fixed";
1075  }
1076  text += ": ";
1077 
1078  std::ostringstream x_ss;
1079  x_ss << std::fixed << std::setprecision(precision);
1080  x_ss << x;
1081  text += x_ss.str().c_str();
1082 
1083  text += ", ";
1084 
1085  std::ostringstream y_ss;
1086  y_ss << std::fixed << std::setprecision(precision);
1087  y_ss << y;
1088  text += y_ss.str().c_str();
1089 
1090  xy_pos_label_->setText(text);
1091  xy_pos_label_->setVisible(true);
1092  xy_pos_label_->update();
1093 
1095  if (tf_manager_->SupportsTransform(
1097  ui_.fixedframe->currentText().toStdString()) &&
1098  tf_manager_->GetTransform(
1100  ui_.fixedframe->currentText().toStdString(),
1101  transform))
1102  {
1103  tf::Vector3 point(x, y, 0);
1104  point = transform * point;
1105 
1106  QString lat_lon_text = "lat/lon: ";
1107 
1108  double lat_scale = (1.0 / 111111.0) * scale;
1109  int32_t lat_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lat_scale))));
1110 
1111  std::ostringstream lat_ss;
1112  lat_ss << std::fixed << std::setprecision(lat_precision);
1113  lat_ss << point.y();
1114  lat_lon_text += lat_ss.str().c_str();
1115 
1116  lat_lon_text += ", ";
1117 
1118  double lon_scale = (1.0 / (111111.0 * std::cos(point.y() * swri_math_util::_deg_2_rad))) * scale;
1119  int32_t lon_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lon_scale))));
1120 
1121  std::ostringstream lon_ss;
1122  lon_ss << std::fixed << std::setprecision(lon_precision);
1123  lon_ss << point.x();
1124  lat_lon_text += lon_ss.str().c_str();
1125 
1126  lat_lon_pos_label_->setText(lat_lon_text);
1127  lat_lon_pos_label_->setVisible(true);
1128  lat_lon_pos_label_->update();
1129  }
1130  else if (lat_lon_pos_label_->isVisible())
1131  {
1132  lat_lon_pos_label_->setVisible(false);
1133  }
1134  }
1135 }
1136 
1138  const std::string& name,
1139  const std::string& type,
1140  bool visible,
1141  bool collapsed,
1142  int draw_order)
1143 {
1144  ConfigItem* config_item = new ConfigItem();
1145 
1146  config_item->SetName(name.c_str());
1147 
1148  std::string real_type = type;
1149  if (real_type == "mapviz_plugins/mutlires_image")
1150  {
1151  // The "multires_image" plugin was originally accidentally named "mutlires_image".
1152  // Loading a mapviz config file that still has the old name would normally cause it
1153  // to crash, so this will check for and correct it.
1154  real_type = "mapviz_plugins/multires_image";
1155  }
1156 
1157 
1158  ROS_INFO("creating: %s", real_type.c_str());
1159  MapvizPluginPtr plugin = loader_->createInstance(real_type.c_str());
1160 
1161  // Setup configure widget
1162  config_item->SetWidget(plugin->GetConfigWidget(this));
1163  plugin->SetIcon(config_item->ui_.icon);
1164  plugin->Initialize(tf_, tf_manager_, canvas_);
1165  plugin->SetType(real_type.c_str());
1166  plugin->SetName(name);
1167  plugin->SetNode(*node_);
1168  plugin->SetVisible(visible);
1169 
1170  if (draw_order == 0)
1171  {
1172  plugin->SetDrawOrder(ui_.configs->count());
1173  }
1174  else if (draw_order > 0)
1175  {
1176  plugin->SetDrawOrder(std::min(ui_.configs->count(), draw_order - 1));
1177  }
1178  else if (draw_order < 0)
1179  {
1180  plugin->SetDrawOrder(std::max(0, ui_.configs->count() + draw_order + 1));
1181  }
1182 
1183  QString pretty_type(real_type.c_str());
1184  pretty_type = pretty_type.split('/').last();
1185  config_item->SetType(pretty_type);
1186  QListWidgetItem* item = new PluginConfigListItem();
1187  config_item->SetListItem(item);
1188  item->setSizeHint(config_item->sizeHint());
1189  connect(config_item, SIGNAL(UpdateSizeHint()), this, SLOT(UpdateSizeHints()));
1190  connect(config_item, SIGNAL(ToggledDraw(QListWidgetItem*, bool)), this, SLOT(ToggleShowPlugin(QListWidgetItem*, bool)));
1191  connect(config_item, SIGNAL(RemoveRequest(QListWidgetItem*)), this, SLOT(RemoveDisplay(QListWidgetItem*)));
1192  connect(plugin.get(), SIGNAL(VisibleChanged(bool)), config_item, SLOT(ToggleDraw(bool)));
1193  connect(plugin.get(), SIGNAL(SizeChanged()), this, SLOT(UpdateSizeHints()));
1194 
1195  if (real_type == "mapviz_plugins/image")
1196  {
1197  // This is a little kludgey because we're relying on hard-coding a
1198  // plugin type here... feel free to suggest a better way.
1199  // If the default image transport has changed, we want to notify all of our
1200  // image plugins of it so that they will resubscribe appropriately.
1201  connect(this, SIGNAL(ImageTransportChanged()),
1202  plugin.get(), SLOT(Resubscribe()));
1203  }
1204 
1205  if (draw_order == 0)
1206  {
1207  ui_.configs->addItem(item);
1208  }
1209  else
1210  {
1211  ui_.configs->insertItem(plugin->DrawOrder(), item);
1212  }
1213 
1214  ui_.configs->setItemWidget(item, config_item);
1215  ui_.configs->UpdateIndices();
1216 
1217  // Add plugin to canvas
1218  plugin->SetTargetFrame(ui_.fixedframe->currentText().toStdString());
1219  plugin->SetUseLatestTransforms(ui_.uselatesttransforms->isChecked());
1220  plugins_[item] = plugin;
1221  canvas_->AddPlugin(plugin, -1);
1222 
1223  config_item->ToggleDraw(visible);
1224 
1225  if (collapsed)
1226  config_item->Hide();
1227 
1228  ReorderDisplays();
1229 
1230  return plugin;
1231 }
1232 
1233 void Mapviz::ToggleShowPlugin(QListWidgetItem* item, bool visible)
1234 {
1235  ROS_INFO("Toggle show plugin");
1236 
1237  if (plugins_.count(item) == 1)
1238  {
1239  plugins_[item]->SetVisible(visible);
1240  }
1241  canvas_->UpdateView();
1242 }
1243 
1244 void Mapviz::FixedFrameSelected(const QString& text)
1245 {
1246  if (!updating_frames_)
1247  {
1248  ROS_INFO("Fixed frame selected: %s", text.toStdString().c_str());
1249  if (canvas_ != NULL)
1250  {
1251  canvas_->SetFixedFrame(text.toStdString().c_str());
1252  }
1253  }
1254 }
1255 
1256 void Mapviz::TargetFrameSelected(const QString& text)
1257 {
1258  if (!updating_frames_)
1259  {
1260  ROS_INFO("Target frame selected: %s", text.toStdString().c_str());
1261 
1262  if (canvas_ != NULL)
1263  {
1264  canvas_->SetTargetFrame(text.toStdString().c_str());
1265  }
1266  }
1267 }
1268 
1270 {
1272 }
1273 
1275 {
1277 }
1278 
1280 {
1281  canvas_->ToggleRotate90(on);
1282 }
1283 
1285 {
1287 }
1288 
1290 {
1291  if (on)
1292  {
1293  ui_.configdock->show();
1294  }
1295  else
1296  {
1297  ui_.configdock->hide();
1298  }
1299 
1300  AdjustWindowSize();
1301 }
1302 
1304 {
1305  ui_.statusbar->setVisible(on);
1306 
1307  AdjustWindowSize();
1308 }
1309 
1311 {
1312  if (on)
1313  {
1314  ui_.actionShow_Status_Bar->setChecked(true);
1315  }
1316 
1317  screenshot_button_->setVisible(on);
1318  rec_button_->setVisible(on);
1319  stop_button_->setVisible(on);
1320  spacer1_->setVisible(on);
1321  spacer2_->setVisible(on);
1322  spacer3_->setVisible(on);
1323 }
1324 
1326 {
1327  stop_button_->setEnabled(true);
1328 
1329  if (on)
1330  {
1331  rec_button_->setIcon(QIcon(":/images/media-playback-pause.png"));
1332  rec_button_->setToolTip("Pause recording video of display canvas");
1333  if (!vid_writer_->isRecording())
1334  {
1335  // Lock the window size.
1336  AdjustWindowSize();
1337 
1338  canvas_->CaptureFrames(true);
1339 
1340  std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
1341  boost::replace_all(posix_time, ".", "_");
1342  std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".avi";
1343  boost::replace_all(filename, "~", getenv("HOME"));
1344 
1345 
1346  if (!vid_writer_->initializeWriter(filename, canvas_->width(), canvas_->height()))
1347  {
1348  ROS_ERROR("Failed to open video file for writing.");
1349  StopRecord();
1350  return;
1351  }
1352 
1353  ROS_INFO("Writing video to: %s", filename.c_str());
1354  ui_.statusbar->showMessage("Recording video to " + QString::fromStdString(filename));
1355 
1356  canvas_->updateGL();
1357  }
1358 
1359  record_timer_.start(1000.0 / 30.0);
1360  }
1361  else
1362  {
1363  rec_button_->setIcon(QIcon(":/images/media-record.png"));
1364  rec_button_->setToolTip("Continue recording video of display canvas");
1365  record_timer_.stop();
1366  }
1367 }
1368 
1369 void Mapviz::SetImageTransport(QAction* transport_action)
1370 {
1371  std::string transport = transport_action->text().toStdString();
1372  ROS_INFO("Setting %s to %s", IMAGE_TRANSPORT_PARAM.c_str(), transport.c_str());
1373  node_->setParam(IMAGE_TRANSPORT_PARAM, transport);
1374 
1375  Q_EMIT(ImageTransportChanged());
1376 }
1377 
1379 {
1380  QList<QAction*> actions = image_transport_menu_->actions();
1381 
1382  std::string current_transport;
1383  node_->param<std::string>(IMAGE_TRANSPORT_PARAM, current_transport, "raw");
1384  Q_FOREACH(QAction* action, actions)
1385  {
1386  if (action->text() == QString::fromStdString(current_transport))
1387  {
1388  action->setChecked(true);
1389  return;
1390  }
1391  }
1392 
1393  ROS_WARN("%s param was set to an unrecognized value: %s",
1394  IMAGE_TRANSPORT_PARAM.c_str(), current_transport.c_str());
1395 }
1396 
1398 {
1399  // We need to store the data inside a QImage in order to emit it as a
1400  // signal.
1401  // Note that the QImage here is set to "ARGB32", but it is actually BGRA.
1402  // Qt doesn't have a comparable BGR format, and the cv::VideoWriter this
1403  // is going to expects BGR format, but it'd be a waste for us to convert
1404  // to RGB and then back to BGR.
1405  QImage frame(canvas_->width(), canvas_->height(), QImage::Format_ARGB32);
1406  if (canvas_->CopyCaptureBuffer(frame.bits()))
1407  {
1408  Q_EMIT(FrameGrabbed(frame));
1409  }
1410  else
1411  {
1412  ROS_ERROR("Failed to get capture buffer");
1413  }
1414 }
1415 
1417 {
1419 }
1420 
1422 {
1423  rec_button_->setChecked(false);
1424  stop_button_->setEnabled(false);
1425 
1426  record_timer_.stop();
1427  if (vid_writer_)
1428  {
1429  vid_writer_->stop();
1430  }
1431  canvas_->CaptureFrames(false);
1432 
1433  ui_.statusbar->showMessage(QString(""));
1434  rec_button_->setToolTip("Start recording video of display canvas");
1435 
1436  AdjustWindowSize();
1437 }
1438 
1440 {
1441  canvas_->CaptureFrame(true);
1442 
1443  std::vector<uint8_t> frame;
1444  if (canvas_->CopyCaptureBuffer(frame))
1445  {
1446  cv::Mat image(canvas_->height(), canvas_->width(), CV_8UC4, &frame[0]);
1447  cv::Mat screenshot;
1448  cvtColor(image, screenshot, cv::COLOR_BGRA2BGR);
1449 
1450  cv::flip(screenshot, screenshot, 0);
1451 
1452  std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
1453  boost::replace_all(posix_time, ".", "_");
1454  std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".png";
1455  boost::replace_all(filename, "~", getenv("HOME"));
1456 
1457  ROS_INFO("Writing screenshot to: %s", filename.c_str());
1458  ui_.statusbar->showMessage("Saved image to " + QString::fromStdString(filename));
1459 
1460  cv::imwrite(filename, screenshot);
1461  }
1462  else
1463  {
1464  ROS_ERROR("Failed to take screenshot.");
1465  }
1466 }
1467 
1469 {
1470  for (int i = 0; i < ui_.configs->count(); i++)
1471  {
1472  QListWidgetItem* item = ui_.configs->item(i);
1473  ConfigItem* widget = static_cast<ConfigItem*>(ui_.configs->itemWidget(item));
1474  if (widget) {
1475  // Make sure the ConfigItem in the QListWidgetItem we're getting really
1476  // exists; if this method is called before it's been initialized, it would
1477  // cause a crash.
1478  item->setSizeHint(widget->sizeHint());
1479  }
1480  }
1481 }
1482 
1484 {
1485  QListWidgetItem* item = ui_.configs->takeItem(ui_.configs->currentRow());
1486  RemoveDisplay(item);
1487 }
1488 
1489 void Mapviz::RemoveDisplay(QListWidgetItem* item)
1490 {
1491  ROS_INFO("Remove display ...");
1492 
1493  if (item)
1494  {
1495  canvas_->RemovePlugin(plugins_[item]);
1496  plugins_.erase(item);
1497 
1498  delete item;
1499  }
1500 }
1501 
1503 {
1504  while (ui_.configs->count() > 0)
1505  {
1506  ROS_INFO("Remove display ...");
1507 
1508  QListWidgetItem* item = ui_.configs->takeItem(0);
1509 
1510  canvas_->RemovePlugin(plugins_[item]);
1511  plugins_.erase(item);
1512 
1513  delete item;
1514  }
1515 }
1516 
1518 {
1519  ROS_INFO("Reorder displays");
1520  for (int i = 0; i < ui_.configs->count(); i++)
1521  {
1522  plugins_[ui_.configs->item(i)]->SetDrawOrder(i);
1523  }
1525 }
1526 
1527 void Mapviz::SelectBackgroundColor(const QColor &color)
1528 {
1529  background_ = color;
1531 }
1532 
1534 {
1535  QFileDialog dialog(this, "Select Capture Directory");
1536  dialog.setFileMode(QFileDialog::DirectoryOnly);
1537 
1538  dialog.exec();
1539 
1540  if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
1541  {
1542  capture_directory_ = dialog.selectedFiles().first().toStdString();
1543  }
1544 }
1545 
1547 {
1548  ROS_INFO("Mapviz Profiling Data");
1549  meas_spin_.printInfo("ROS SpinOnce()");
1550  for (auto& display: plugins_)
1551  {
1552  MapvizPluginPtr plugin = display.second;
1553  if (plugin)
1554  {
1555  plugin->PrintMeasurements();
1556  }
1557  }
1558 }
1559 }
void SetWidget(QWidget *widget)
Definition: config_item.cpp:90
VideoWriter * vid_writer_
Definition: mapviz.h:171
void ToggleShowPlugin(QListWidgetItem *item, bool visible)
Definition: mapviz.cpp:1233
void SaveConfigAs()
Definition: mapviz.cpp:908
#define NULL
void CaptureFrames(bool enabled)
Definition: map_canvas.h:119
string name
bool force_480p_
Definition: mapviz.h:163
void FixedFrameSelected(const QString &text)
Definition: mapviz.cpp:1244
static const long double _deg_2_rad
filename
void AdjustWindowSize()
Definition: mapviz.cpp:479
void SpinOnce()
Definition: mapviz.cpp:339
QColor background_
Definition: mapviz.h:165
void FrameGrabbed(QImage)
MapCanvas * canvas_
Definition: mapviz.h:181
void OpenConfig()
Definition: mapviz.cpp:875
bool LoadFile(const std::string &path, YAML::Node &yaml)
bool initialized_
Definition: mapviz.h:161
void HandleProfileTimer()
Definition: mapviz.cpp:1546
static const std::string _wgs84_frame
void RemovePlugin(MapvizPluginPtr plugin)
Definition: map_canvas.cpp:465
Ui::configitem ui_
Definition: config_item.h:64
QPushButton * stop_button_
Definition: mapviz.h:154
void ToggleUseLatestTransforms(bool on)
Definition: mapviz.cpp:1269
bool resizable_
Definition: mapviz.h:164
QLabel * xy_pos_label_
Definition: mapviz.h:146
void UpdateSizeHints()
Definition: mapviz.cpp:1468
bool LoadMap(const std::map< std::string, std::string > &dict, YAML::Node &yaml)
void CaptureFrame(bool force=false)
Definition: map_canvas.cpp:183
bool initializeWriter(const std::string &directory, int width, int height)
float OffsetX() const
Definition: map_canvas.h:84
void Recenter()
Definition: mapviz.cpp:1416
QWidget * spacer1_
Definition: mapviz.h:149
QTimer save_timer_
Definition: mapviz.h:142
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
config
void InitializeTf(boost::shared_ptr< tf::TransformListener > tf)
Definition: map_canvas.cpp:103
void CaptureVideoFrame()
Definition: mapviz.cpp:1397
void ToggleEnableAntialiasing(bool on)
Definition: map_canvas.cpp:441
void SetFixedFrame(const std::string &frame)
Definition: map_canvas.cpp:411
QPushButton * recenter_button_
Definition: mapviz.h:152
float OffsetY() const
Definition: map_canvas.h:85
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void ToggleRecord(bool on)
Definition: mapviz.cpp:1325
void ToggleCaptureTools(bool on)
Definition: mapviz.cpp:1310
void StopRecord()
Definition: mapviz.cpp:1421
void ClearDisplays()
Definition: mapviz.cpp:1502
#define ROS_WARN(...)
void SetBackground(const QColor &color)
Definition: map_canvas.h:113
QWidget * spacer2_
Definition: mapviz.h:150
void UpdateImageTransportMenu()
Definition: mapviz.cpp:1378
virtual void closeEvent(QCloseEvent *event)
Definition: mapviz.cpp:218
pluginlib::ClassLoader< MapvizPlugin > * loader_
Definition: mapviz.h:180
void ToggleEnableAntialiasing(bool on)
Definition: mapviz.cpp:1284
void printInfo(const std::string &name) const
Definition: stopwatch.h:87
void Force480p(bool on)
Definition: mapviz.cpp:447
Stopwatch meas_spin_
Definition: mapviz.h:184
MapvizPluginPtr CreateNewDisplay(const std::string &name, const std::string &type, bool visible, bool collapsed, int draw_order=0)
Definition: mapviz.cpp:1137
void ToggleDraw(bool toggled)
Definition: config_item.cpp:56
virtual void showEvent(QShowEvent *event)
Definition: mapviz.cpp:213
std::map< QListWidgetItem *, MapvizPluginPtr > plugins_
Definition: mapviz.h:182
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void ToggleRotate90(bool on)
Definition: mapviz.cpp:1279
swri_transform_util::TransformManagerPtr tf_manager_
Definition: mapviz.h:178
ros::ServiceServer add_display_srv_
Definition: mapviz.h:176
void ImageTransportChanged()
void SetResizable(bool on)
Definition: mapviz.cpp:463
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
void Force720p(bool on)
Definition: mapviz.cpp:431
QTimer spin_timer_
Definition: mapviz.h:141
void ClearConfig()
Definition: mapviz.cpp:892
bool FindValue(const YAML::Node &node, const std::string &name)
bool is_standalone_
Definition: mapviz.h:160
void Save(const std::string &filename)
Definition: mapviz.cpp:764
void ToggleConfigPanel(bool on)
Definition: mapviz.cpp:1289
void Screenshot()
Definition: mapviz.cpp:1439
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
QTimer profile_timer_
Definition: mapviz.h:144
char ** argv_
Definition: mapviz.h:158
void SetName(QString name)
Definition: config_item.cpp:78
QWidget * spacer3_
Definition: mapviz.h:151
ROSCPP_DECL bool ok()
void ReorderDisplays()
Definition: mapviz.cpp:1517
void SaveConfig()
Definition: mapviz.cpp:897
void ClearHistory()
Definition: mapviz.cpp:941
void ToggleRotate90(bool on)
Definition: map_canvas.cpp:436
void SelectNewDisplay()
Definition: mapviz.cpp:950
static const std::string IMAGE_TRANSPORT_PARAM
Definition: mapviz.h:208
boost::shared_ptr< tf::TransformListener > tf_
Definition: mapviz.h:177
void TargetFrameSelected(const QString &text)
Definition: mapviz.cpp:1256
void Initialize()
Definition: mapviz.cpp:231
std::string capture_directory_
Definition: mapviz.h:169
void ToggleFixOrientation(bool on)
Definition: mapviz.cpp:1274
action
Mapviz(bool is_standalone, int argc, char **argv, QWidget *parent=0, Qt::WindowFlags flags=0)
Definition: mapviz.cpp:87
QLabel * lat_lon_pos_label_
Definition: mapviz.h:147
Ui::mapviz ui_
Definition: mapviz.h:136
void SetListItem(QListWidgetItem *item)
Definition: config_item.h:60
void SetOffsetY(float y)
Definition: map_canvas.h:107
void SelectBackgroundColor(const QColor &color)
Definition: mapviz.cpp:1527
void Open(const std::string &filename)
Definition: mapviz.cpp:524
static const QString MAPVIZ_CONFIG_FILE
Definition: mapviz.h:207
static const QString ROS_WORKSPACE_VAR
Definition: mapviz.h:206
QTimer frame_timer_
Definition: mapviz.h:140
QPushButton * rec_button_
Definition: mapviz.h:153
QThread video_thread_
Definition: mapviz.h:170
bool CopyCaptureBuffer(uchar *buffer)
Definition: map_canvas.h:132
void SetOffsetX(float x)
Definition: map_canvas.h:101
bool AddDisplay(AddMapvizDisplay::Request &req, AddMapvizDisplay::Response &resp)
Definition: mapviz.cpp:989
void ToggleUseLatestTransforms(bool on)
Definition: map_canvas.cpp:451
static WallTime now()
void SetType(QString type)
Definition: config_item.cpp:84
void SetTargetFrame(const std::string &frame)
Definition: map_canvas.cpp:421
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
ros::NodeHandle * node_
Definition: mapviz.h:175
void SetCaptureDirectory()
Definition: mapviz.cpp:1533
void Hover(double x, double y, double scale)
Definition: mapviz.cpp:1058
void ToggleFixOrientation(bool on)
Definition: map_canvas.cpp:431
QTimer record_timer_
Definition: mapviz.h:143
bool updating_frames_
Definition: mapviz.h:173
void SetImageTransport(QAction *transport_action)
Definition: mapviz.cpp:1369
void RemoveDisplay()
Definition: mapviz.cpp:1483
void UpdateFrames()
Definition: mapviz.cpp:353
void SetViewScale(float scale)
Definition: map_canvas.h:95
ROSCPP_DECL void spinOnce()
void AutoSave()
Definition: mapviz.cpp:841
float ViewScale() const
Definition: map_canvas.h:83
void AddPlugin(MapvizPluginPtr plugin, int order)
Definition: map_canvas.cpp:460
#define ROS_ERROR(...)
QPushButton * screenshot_button_
Definition: mapviz.h:155
std::string save_location_
Definition: mapviz.h:167
void ToggleStatusBar(bool on)
Definition: mapviz.cpp:1303
bool force_720p_
Definition: mapviz.h:162
QMenu * image_transport_menu_
Definition: mapviz.h:138
#define ROS_DEBUG(...)
std::vector< std::string > getLoadableTransports() const


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