mapviz.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <mapviz/mapviz.h>
00031 
00032 // C++ standard libraries
00033 #include <cmath>
00034 #include <cstdlib>
00035 #include <algorithm>
00036 #include <fstream>
00037 #include <sstream>
00038 
00039 // Boost libraries
00040 #include <boost/algorithm/string/replace.hpp>
00041 #include <boost/algorithm/string/split.hpp>
00042 #include <boost/algorithm/string/join.hpp>
00043 #include <boost/date_time/posix_time/posix_time.hpp>
00044 #include <boost/filesystem.hpp>
00045 #include <boost/make_shared.hpp>
00046 
00047 #include <opencv2/core/core.hpp>
00048 #include <opencv2/imgproc/imgproc.hpp>
00049 
00050 // QT libraries
00051 #if QT_VERSION >= 0x050000
00052 #include <QtWidgets/QApplication>
00053 #else
00054 #include <QtGui/QApplication>
00055 #endif
00056 #include <QFileDialog>
00057 #include <QActionGroup>
00058 #include <QColorDialog>
00059 #include <QLabel>
00060 #include <QMessageBox>
00061 #include <QProcessEnvironment>
00062 #include <QFileInfo>
00063 #include <QListWidgetItem>
00064 #include <QMutexLocker>
00065 
00066 #include <swri_math_util/constants.h>
00067 #include <swri_transform_util/frames.h>
00068 #include <swri_yaml_util/yaml_util.h>
00069 
00070 #include <mapviz/config_item.h>
00071 #include <QtGui/QtGui>
00072 
00073 #include <image_transport/image_transport.h>
00074 
00075 namespace mapviz
00076 {
00077 const QString Mapviz::ROS_WORKSPACE_VAR = "ROS_WORKSPACE";
00078 const QString Mapviz::MAPVIZ_CONFIG_FILE = "/.mapviz_config";
00079 const std::string Mapviz::IMAGE_TRANSPORT_PARAM = "image_transport";
00080 
00081 Mapviz::Mapviz(bool is_standalone, int argc, char** argv, QWidget *parent, Qt::WFlags flags) :
00082     QMainWindow(parent, flags),
00083     xy_pos_label_(new QLabel("fixed: 0.0,0.0")),
00084     lat_lon_pos_label_(new QLabel("lat/lon: 0.0,0.0")),
00085     argc_(argc),
00086     argv_(argv),
00087     is_standalone_(is_standalone),
00088     initialized_(false),
00089     force_720p_(false),
00090     force_480p_(false),
00091     resizable_(true),
00092     background_(Qt::gray),
00093     capture_directory_("~"),
00094     vid_writer_(NULL),
00095     updating_frames_(false),
00096     node_(NULL),
00097     canvas_(NULL)
00098 {
00099   ui_.setupUi(this);
00100 
00101   xy_pos_label_->setVisible(false);
00102   lat_lon_pos_label_->setVisible(false);
00103 
00104   ui_.statusbar->addPermanentWidget(xy_pos_label_);
00105   ui_.statusbar->addPermanentWidget(lat_lon_pos_label_);
00106 
00107   spacer1_ = new QWidget(ui_.statusbar);
00108   spacer1_->setMaximumSize(22,22);
00109   spacer1_->setMinimumSize(22,22);
00110   ui_.statusbar->addPermanentWidget(spacer1_);
00111 
00112   screenshot_button_ = new QPushButton();
00113   screenshot_button_->setMinimumSize(22, 22);
00114   screenshot_button_->setMaximumSize(22,22);
00115   screenshot_button_->setIcon(QIcon(":/images/image-x-generic.png"));
00116   screenshot_button_->setFlat(true);
00117   screenshot_button_->setToolTip("Capture screenshot of display canvas");
00118   ui_.statusbar->addPermanentWidget(screenshot_button_);
00119 
00120   spacer2_ = new QWidget(ui_.statusbar);
00121   spacer2_->setMaximumSize(22,22);
00122   spacer2_->setMinimumSize(22,22);
00123   ui_.statusbar->addPermanentWidget(spacer2_);
00124 
00125   rec_button_ = new QPushButton();
00126   rec_button_->setMinimumSize(22, 22);
00127   rec_button_->setMaximumSize(22,22);
00128   rec_button_->setIcon(QIcon(":/images/media-record.png"));
00129   rec_button_->setCheckable(true);
00130   rec_button_->setFlat(true);
00131   rec_button_->setToolTip("Start recording video of display canvas");
00132   ui_.statusbar->addPermanentWidget(rec_button_);
00133 
00134   stop_button_ = new QPushButton();
00135   stop_button_->setMinimumSize(22, 22);
00136   stop_button_->setMaximumSize(22,22);
00137   stop_button_->setIcon(QIcon(":/images/media-playback-stop.png"));
00138   stop_button_->setToolTip("Stop recording video of display canvas");
00139   stop_button_->setEnabled(false);
00140   stop_button_->setFlat(true);
00141   ui_.statusbar->addPermanentWidget(stop_button_);
00142 
00143   spacer3_ = new QWidget(ui_.statusbar);
00144   spacer3_->setMaximumSize(22,22);
00145   spacer3_->setMinimumSize(22,22);
00146   ui_.statusbar->addPermanentWidget(spacer3_);
00147 
00148   recenter_button_ = new QPushButton();
00149   recenter_button_->setMinimumSize(22, 22);
00150   recenter_button_->setMaximumSize(22, 22);
00151   recenter_button_->setIcon(QIcon(":/images/arrow_in.png"));
00152   recenter_button_->setToolTip("Reset the viewport to the default location and zoom level");
00153   recenter_button_->setFlat(true);
00154   ui_.statusbar->addPermanentWidget(recenter_button_);
00155 
00156   ui_.statusbar->setVisible(true);
00157 
00158   QActionGroup* group = new QActionGroup(this);
00159 
00160   ui_.actionForce_720p->setActionGroup(group);
00161   ui_.actionForce_480p->setActionGroup(group);
00162   ui_.actionResizable->setActionGroup(group);
00163 
00164   ui_.targetframe->addItem("<none>");
00165 
00166   canvas_ = new MapCanvas(this);
00167   setCentralWidget(canvas_);
00168 
00169   connect(canvas_, SIGNAL(Hover(double,double,double)), this, SLOT(Hover(double,double,double)));
00170   connect(ui_.configs, SIGNAL(ItemsMoved()), this, SLOT(ReorderDisplays()));
00171   connect(ui_.actionExit, SIGNAL(triggered()), this, SLOT(close()));
00172   connect(ui_.bg_color, SIGNAL(colorEdited(const QColor &)), this, SLOT(SelectBackgroundColor(const QColor &)));
00173 
00174   connect(recenter_button_, SIGNAL(clicked()), this, SLOT(Recenter()));
00175   connect(rec_button_, SIGNAL(toggled(bool)), this, SLOT(ToggleRecord(bool)));
00176   connect(stop_button_, SIGNAL(clicked()), this, SLOT(StopRecord()));
00177   connect(screenshot_button_, SIGNAL(clicked()), this, SLOT(Screenshot()));
00178   connect(ui_.actionClear_History, SIGNAL(triggered()), this, SLOT(ClearHistory()));
00179 
00180   // Use a separate thread for writing video files so that it won't cause
00181   // lag on the main thread.
00182   // It's ok for the video writer to be a pointer that we intantiate here and
00183   // then forget about; the worker thread will delete it when the thread exits.
00184   vid_writer_ = new VideoWriter();
00185   vid_writer_->moveToThread(&video_thread_);
00186   connect(&video_thread_, SIGNAL(finished()), vid_writer_, SLOT(deleteLater()));
00187   connect(this, SIGNAL(FrameGrabbed(QImage)), vid_writer_, SLOT(processFrame(QImage)));
00188   video_thread_.start();
00189 
00190   image_transport_menu_ = new QMenu("Default Image Transport", ui_.menu_View);
00191   ui_.menu_View->addMenu(image_transport_menu_);
00192 
00193   connect(image_transport_menu_, SIGNAL(aboutToShow()), this, SLOT(UpdateImageTransportMenu()));
00194 
00195   ui_.bg_color->setColor(background_);
00196   canvas_->SetBackground(background_);
00197 }
00198 
00199 Mapviz::~Mapviz()
00200 {
00201   video_thread_.quit();
00202   video_thread_.wait();
00203   delete node_;
00204 }
00205 
00206 void Mapviz::showEvent(QShowEvent* event)
00207 {
00208   Initialize();
00209 }
00210 
00211 void Mapviz::closeEvent(QCloseEvent* event)
00212 {
00213   AutoSave();
00214 
00215   for (auto& display: plugins_)
00216   {
00217     MapvizPluginPtr plugin = display.second;
00218     canvas_->RemovePlugin(plugin);
00219   }
00220 
00221   plugins_.clear();
00222 }
00223 
00224 void Mapviz::Initialize()
00225 {
00226   if (!initialized_)
00227   {
00228     if (is_standalone_)
00229     {
00230       // If this Mapviz is running as a standalone application, it needs to init
00231       // ROS and start spinning.  If it's running as an rqt plugin, rqt will
00232       // take care of that.
00233       ros::init(argc_, argv_, "mapviz", ros::init_options::AnonymousName);
00234 
00235       spin_timer_.start(30);
00236       connect(&spin_timer_, SIGNAL(timeout()), this, SLOT(SpinOnce()));
00237     }
00238 
00239     node_ = new ros::NodeHandle("~");
00240 
00241     // Create a sub-menu that lists all available Image Transports
00242     image_transport::ImageTransport it(*node_);
00243     std::vector<std::string> transports = it.getLoadableTransports();
00244     QActionGroup* group = new QActionGroup(image_transport_menu_);
00245     for (std::vector<std::string>::iterator iter = transports.begin(); iter != transports.end(); iter++)
00246     {
00247       QString transport = QString::fromStdString(*iter).replace(
00248           QString::fromStdString(IMAGE_TRANSPORT_PARAM) + "/", "");
00249       QAction* action = image_transport_menu_->addAction(transport);
00250       action->setCheckable(true);
00251       group->addAction(action);
00252     }
00253 
00254     connect(group, SIGNAL(triggered(QAction*)), this, SLOT(SetImageTransport(QAction*)));
00255 
00256     tf_ = boost::make_shared<tf::TransformListener>();
00257     tf_manager_.Initialize(tf_);
00258 
00259     loader_ = new pluginlib::ClassLoader<MapvizPlugin>(
00260         "mapviz", "mapviz::MapvizPlugin");
00261 
00262     std::vector<std::string> plugins = loader_->getDeclaredClasses();
00263     for (unsigned int i = 0; i < plugins.size(); i++)
00264     {
00265       ROS_INFO("Found mapviz plugin: %s", plugins[i].c_str());
00266     }
00267 
00268     canvas_->InitializeTf(tf_);
00269     canvas_->SetFixedFrame(ui_.fixedframe->currentText().toStdString());
00270     canvas_->SetTargetFrame(ui_.targetframe->currentText().toStdString());
00271 
00272     ros::NodeHandle priv("~");
00273 
00274     add_display_srv_ = node_->advertiseService("add_mapviz_display", &Mapviz::AddDisplay, this);
00275 
00276     QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
00277     QString default_path = QDir::homePath();
00278     if (env.contains(ROS_WORKSPACE_VAR))
00279     {
00280       // If the ROS_WORKSPACE environment variable is defined, try to read our
00281       // config file out of that.  If we can't read it, fall back to trying to
00282       // read one from the user's home directory.
00283       QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
00284       if (QFileInfo(ws_path + MAPVIZ_CONFIG_FILE).isReadable())
00285       {
00286         default_path = ws_path;
00287       }
00288       else
00289       {
00290         ROS_WARN("Could not load config file from ROS_WORKSPACE at %s; trying home directory...",
00291                  ws_path.toStdString().c_str());
00292       }
00293     }
00294     default_path += MAPVIZ_CONFIG_FILE;
00295 
00296 
00297     std::string config;
00298     priv.param("config", config, default_path.toStdString());
00299 
00300     bool auto_save;
00301     priv.param("auto_save_backup", auto_save, true);
00302 
00303     Open(config);
00304 
00305     UpdateFrames();
00306     frame_timer_.start(1000);
00307     connect(&frame_timer_, SIGNAL(timeout()), this, SLOT(UpdateFrames()));
00308 
00309     if (auto_save)
00310     {
00311       save_timer_.start(10000);
00312       connect(&save_timer_, SIGNAL(timeout()), this, SLOT(AutoSave()));
00313     }
00314 
00315     connect(&record_timer_, SIGNAL(timeout()), this, SLOT(CaptureVideoFrame()));
00316 
00317     bool print_profile_data;
00318     priv.param("print_profile_data", print_profile_data, false);
00319     if (print_profile_data)
00320     {
00321       profile_timer_.start(2000);
00322       connect(&profile_timer_, SIGNAL(timeout()), this, SLOT(HandleProfileTimer()));
00323     }
00324 
00325     initialized_ = true;
00326   }
00327 }
00328 
00329 void Mapviz::SpinOnce()
00330 {
00331   if (ros::ok())
00332   {
00333     meas_spin_.start();
00334     ros::spinOnce();
00335     meas_spin_.stop();
00336   }
00337   else
00338   {
00339     QApplication::exit();
00340   }
00341 }
00342 
00343 void Mapviz::UpdateFrames()
00344 {
00345   std::vector<std::string> frames;
00346   tf_->getFrameStrings(frames);
00347   std::sort(frames.begin(), frames.end());
00348 
00349   if (ui_.fixedframe->count() >= 0 &&
00350       static_cast<size_t>(ui_.fixedframe->count()) == frames.size())
00351   {
00352     bool changed = false;
00353     for (size_t i = 0; i < frames.size(); i++)
00354     {
00355       if (frames[i] != ui_.fixedframe->itemText(i).toStdString())
00356       {
00357         changed = true;
00358       }
00359     }
00360 
00361     if (!changed)
00362       return;
00363   }
00364 
00365   updating_frames_ = true;
00366 
00367   std::string current_fixed = ui_.fixedframe->currentText().toStdString();
00368 
00369   ui_.fixedframe->clear();
00370   for (size_t i = 0; i < frames.size(); i++)
00371   {
00372     ui_.fixedframe->addItem(frames[i].c_str());
00373   }
00374 
00375   if (current_fixed != "")
00376   {
00377     int index = ui_.fixedframe->findText(current_fixed.c_str());
00378     if (index < 0)
00379     {
00380       ui_.fixedframe->addItem(current_fixed.c_str());
00381     }
00382 
00383     index = ui_.fixedframe->findText(current_fixed.c_str());
00384     ui_.fixedframe->setCurrentIndex(index);
00385   }
00386 
00387   std::string current_target = ui_.targetframe->currentText().toStdString();
00388 
00389   ui_.targetframe->clear();
00390   ui_.targetframe->addItem("<none>");
00391   for (size_t i = 0; i < frames.size(); i++)
00392   {
00393     ui_.targetframe->addItem(frames[i].c_str());
00394   }
00395 
00396   if (current_target != "")
00397   {
00398     int index = ui_.targetframe->findText(current_target.c_str());
00399     if (index < 0)
00400     {
00401       ui_.targetframe->addItem(current_target.c_str());
00402     }
00403 
00404     index = ui_.targetframe->findText(current_target.c_str());
00405     ui_.targetframe->setCurrentIndex(index);
00406   }
00407 
00408   updating_frames_ = false;
00409 
00410   if (current_target != ui_.targetframe->currentText().toStdString())
00411   {
00412     TargetFrameSelected(ui_.targetframe->currentText());
00413   }
00414 
00415   if (current_fixed != ui_.fixedframe->currentText().toStdString())
00416   {
00417     FixedFrameSelected(ui_.fixedframe->currentText());
00418   }
00419 }
00420 
00421 void Mapviz::Force720p(bool on)
00422 {
00423   if (force_720p_ != on)
00424   {
00425     force_720p_ = on;
00426 
00427     if (force_720p_)
00428     {
00429       force_480p_ = false;
00430       resizable_ = false;
00431     }
00432 
00433     AdjustWindowSize();
00434   }
00435 }
00436 
00437 void Mapviz::Force480p(bool on)
00438 {
00439   if (force_480p_ != on)
00440   {
00441     force_480p_ = on;
00442 
00443     if (force_480p_)
00444     {
00445       force_720p_ = false;
00446       resizable_ = false;
00447     }
00448 
00449     AdjustWindowSize();
00450   }
00451 }
00452 
00453 void Mapviz::SetResizable(bool on)
00454 {
00455   if (resizable_ != on)
00456   {
00457     resizable_ = on;
00458 
00459     if (resizable_)
00460     {
00461       force_720p_ = false;
00462       force_480p_ = false;
00463     }
00464 
00465     AdjustWindowSize();
00466   }
00467 }
00468 
00469 void Mapviz::AdjustWindowSize()
00470 {
00471   canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred));
00472   setSizePolicy(QSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred));
00473 
00474   this->setMinimumSize(QSize(100, 100));
00475   this->setMaximumSize(QSize(10000, 10000));
00476 
00477   if (force_720p_)
00478   {
00479     canvas_->setMinimumSize(1280, 720);
00480     canvas_->setMaximumSize(1280, 720);
00481     canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00482     adjustSize();
00483     this->setMaximumSize(this->sizeHint());
00484     this->setMinimumSize(this->sizeHint());
00485     setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00486   }
00487   else if (force_480p_)
00488   {
00489     canvas_->setMinimumSize(640, 480);
00490     canvas_->setMaximumSize(640, 480);
00491     canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00492     adjustSize();
00493     this->setMaximumSize(this->sizeHint());
00494     this->setMinimumSize(this->sizeHint());
00495     setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00496   }
00497   else if (stop_button_->isEnabled())
00498   {
00499     canvas_->setMinimumSize(canvas_->width(), canvas_->height());
00500     canvas_->setMaximumSize(canvas_->width(), canvas_->height());
00501     canvas_->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00502     adjustSize();
00503     this->setMaximumSize(this->sizeHint());
00504     this->setMinimumSize(this->sizeHint());
00505     setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
00506   }
00507   else
00508   {
00509     canvas_->setMinimumSize(100, 100);
00510     canvas_->setMaximumSize(10000, 10000);
00511   }
00512 }
00513 
00514 void Mapviz::Open(const std::string& filename)
00515 {
00516   ROS_INFO("Loading configuration from: %s", filename.c_str());
00517 
00518   std::string title;
00519   size_t last_slash = filename.find_last_of('/');
00520   if (last_slash != std::string::npos && last_slash != filename.size() - 1)
00521   {
00522     title = filename.substr(last_slash + 1) + " (" +
00523             filename.substr(0, last_slash + 1) + ")";
00524   }
00525   else
00526   {
00527     title = filename;
00528   }
00529 
00530   title += " - mapviz";
00531   setWindowTitle(QString::fromStdString(title));
00532 
00533   YAML::Node doc;
00534   if (!swri_yaml_util::LoadFile(filename, doc))
00535   {
00536     ROS_ERROR("Failed to load file: %s", filename.c_str());
00537     return;
00538   }
00539 
00540   std::vector<std::string> failed_plugins;
00541 
00542   try
00543   {
00544     boost::filesystem::path filepath(filename);
00545     std::string config_path = filepath.parent_path().string();
00546 
00547     ClearDisplays();
00548 
00549     if (swri_yaml_util::FindValue(doc, "capture_directory"))
00550     {
00551       doc["capture_directory"] >> capture_directory_;
00552     }
00553 
00554     if (swri_yaml_util::FindValue(doc, "fixed_frame"))
00555     {
00556       std::string fixed_frame;
00557       doc["fixed_frame"] >> fixed_frame;
00558       ui_.fixedframe->setEditText(fixed_frame.c_str());
00559     }
00560 
00561     if (swri_yaml_util::FindValue(doc, "target_frame"))
00562     {
00563       std::string target_frame;
00564       doc["target_frame"] >> target_frame;
00565       ui_.targetframe->setEditText(target_frame.c_str());
00566     }
00567 
00568     if (swri_yaml_util::FindValue(doc, "fix_orientation"))
00569     {
00570       bool fix_orientation = false;
00571       doc["fix_orientation"] >> fix_orientation;
00572       ui_.actionFix_Orientation->setChecked(fix_orientation);
00573     }
00574 
00575     if (swri_yaml_util::FindValue(doc, "rotate_90"))
00576     {
00577       bool rotate_90 = false;
00578       doc["rotate_90"] >> rotate_90;
00579       ui_.actionRotate_90->setChecked(rotate_90);
00580     }
00581 
00582     if (swri_yaml_util::FindValue(doc, "enable_antialiasing"))
00583     {
00584       bool enable_antialiasing = true;
00585       doc["enable_antialiasing"] >> enable_antialiasing;
00586       ui_.actionEnable_Antialiasing->setChecked(enable_antialiasing);
00587     }
00588 
00589     if (swri_yaml_util::FindValue(doc, "show_displays"))
00590     {
00591       bool show_displays = false;
00592       doc["show_displays"] >> show_displays;
00593       ui_.actionConfig_Dock->setChecked(show_displays);
00594     }
00595 
00596     if (swri_yaml_util::FindValue(doc, "show_capture_tools"))
00597     {
00598       bool show_capture_tools = false;
00599       doc["show_capture_tools"] >> show_capture_tools;
00600       ui_.actionShow_Capture_Tools->setChecked(show_capture_tools);
00601     }
00602 
00603     if (swri_yaml_util::FindValue(doc, "show_status_bar"))
00604     {
00605       bool show_status_bar = false;
00606       doc["show_status_bar"] >> show_status_bar;
00607       ui_.actionShow_Status_Bar->setChecked(show_status_bar);
00608     }
00609 
00610     if (swri_yaml_util::FindValue(doc, "show_capture_tools"))
00611     {
00612       bool show_capture_tools = false;
00613       doc["show_capture_tools"] >> show_capture_tools;
00614       ui_.actionShow_Capture_Tools->setChecked(show_capture_tools);
00615     }
00616 
00617     if (swri_yaml_util::FindValue(doc, "window_width"))
00618     {
00619       int window_width = 0;
00620       doc["window_width"] >> window_width;
00621       resize(window_width, height());
00622     }
00623 
00624     if (swri_yaml_util::FindValue(doc, "window_height"))
00625     {
00626       int window_height = 0;
00627       doc["window_height"] >> window_height;
00628       resize(width(), window_height);
00629     }
00630 
00631     if (swri_yaml_util::FindValue(doc, "view_scale"))
00632     {
00633       float scale = 0;
00634       doc["view_scale"] >> scale;
00635       canvas_->SetViewScale(scale);
00636     }
00637 
00638     if (swri_yaml_util::FindValue(doc, "offset_x"))
00639     {
00640       float x = 0;
00641       doc["offset_x"] >> x;
00642       canvas_->SetOffsetX(x);
00643     }
00644 
00645     if (swri_yaml_util::FindValue(doc, "offset_x"))
00646     {
00647       float y = 0;
00648       doc["offset_y"] >> y;
00649       canvas_->SetOffsetY(y);
00650     }
00651 
00652     if (swri_yaml_util::FindValue(doc, "force_720p"))
00653     {
00654       bool force_720p;
00655       doc["force_720p"] >> force_720p;
00656 
00657       if (force_720p)
00658       {
00659         ui_.actionForce_720p->setChecked(true);
00660       }
00661     }
00662 
00663     if (swri_yaml_util::FindValue(doc, "force_480p"))
00664     {
00665       bool force_480p;
00666       doc["force_480p"] >> force_480p;
00667 
00668       if (force_480p)
00669       {
00670         ui_.actionForce_480p->setChecked(true);
00671       }
00672     }
00673 
00674     if (swri_yaml_util::FindValue(doc, IMAGE_TRANSPORT_PARAM))
00675     {
00676       std::string image_transport;
00677       doc[IMAGE_TRANSPORT_PARAM] >> image_transport;
00678 
00679       node_->setParam(IMAGE_TRANSPORT_PARAM, image_transport);
00680     }
00681 
00682     bool use_latest_transforms = true;
00683     if (swri_yaml_util::FindValue(doc, "use_latest_transforms"))
00684     {
00685       doc["use_latest_transforms"] >> use_latest_transforms;
00686     }
00687     ui_.uselatesttransforms->setChecked(use_latest_transforms);
00688     canvas_->ToggleUseLatestTransforms(use_latest_transforms);
00689 
00690     if (swri_yaml_util::FindValue(doc, "background"))
00691     {
00692       std::string color;
00693       doc["background"] >> color;
00694       background_ = QColor(color.c_str());
00695       ui_.bg_color->setColor(background_);
00696       canvas_->SetBackground(background_);
00697     }
00698 
00699     if (swri_yaml_util::FindValue(doc, "displays"))
00700     {
00701       const YAML::Node& displays = doc["displays"];
00702       for (uint32_t i = 0; i < displays.size(); i++)
00703       {
00704         std::string type, name;
00705         displays[i]["type"] >> type;
00706         displays[i]["name"] >> name;
00707 
00708         const YAML::Node& config = displays[i]["config"];
00709 
00710         bool visible = false;
00711         config["visible"] >> visible;
00712 
00713         bool collapsed = false;
00714         config["collapsed"] >> collapsed;
00715 
00716         try
00717         {
00718           MapvizPluginPtr plugin =
00719               CreateNewDisplay(name, type, visible, collapsed);
00720           plugin->LoadConfig(config, config_path);
00721           plugin->DrawIcon();
00722         }
00723         catch (const pluginlib::LibraryLoadException& e)
00724         {
00725           failed_plugins.push_back(type);
00726           ROS_ERROR("%s", e.what());
00727         }
00728       }
00729     }
00730   }
00731   catch (const YAML::ParserException& e)
00732   {
00733     ROS_ERROR("%s", e.what());
00734     return;
00735   }
00736   catch (const YAML::Exception& e)
00737   {
00738     ROS_ERROR("%s", e.what());
00739     return;
00740   }
00741 
00742   if (!failed_plugins.empty())
00743   {
00744     std::stringstream message;
00745     message << "The following plugin(s) failed to load:" << std::endl;
00746     std::string failures = boost::algorithm::join(failed_plugins, "\n");
00747     message << failures << std::endl << std::endl << "Check the ROS log for more details.";
00748 
00749     QMessageBox::warning(this, "Failed to load plugins", QString::fromStdString(message.str()));
00750   }
00751 }
00752 
00753 void Mapviz::Save(const std::string& filename)
00754 {
00755   std::ofstream fout(filename.c_str());
00756   if (fout.fail())
00757   {
00758     ROS_ERROR("Failed to open file: %s", filename.c_str());
00759     return;
00760   }
00761 
00762   boost::filesystem::path filepath(filename);
00763   std::string config_path = filepath.parent_path().string();
00764 
00765   YAML::Emitter out;
00766 
00767   out << YAML::BeginMap;
00768   out << YAML::Key << "capture_directory" << YAML::Value << capture_directory_;
00769   out << YAML::Key << "fixed_frame" << YAML::Value << ui_.fixedframe->currentText().toStdString();
00770   out << YAML::Key << "target_frame" << YAML::Value << ui_.targetframe->currentText().toStdString();
00771   out << YAML::Key << "fix_orientation" << YAML::Value << ui_.actionFix_Orientation->isChecked();
00772   out << YAML::Key << "rotate_90" << YAML::Value << ui_.actionRotate_90->isChecked();
00773   out << YAML::Key << "enable_antialiasing" << YAML::Value << ui_.actionEnable_Antialiasing->isChecked();
00774   out << YAML::Key << "show_displays" << YAML::Value << ui_.actionConfig_Dock->isChecked();
00775   out << YAML::Key << "show_status_bar" << YAML::Value << ui_.actionShow_Status_Bar->isChecked();
00776   out << YAML::Key << "show_capture_tools" << YAML::Value << ui_.actionShow_Capture_Tools->isChecked();
00777   out << YAML::Key << "window_width" << YAML::Value << width();
00778   out << YAML::Key << "window_height" << YAML::Value << height();
00779   out << YAML::Key << "view_scale" << YAML::Value << canvas_->ViewScale();
00780   out << YAML::Key << "offset_x" << YAML::Value << canvas_->OffsetX();
00781   out << YAML::Key << "offset_y" << YAML::Value << canvas_->OffsetY();
00782   out << YAML::Key << "use_latest_transforms" << YAML::Value << ui_.uselatesttransforms->isChecked();
00783   out << YAML::Key << "background" << YAML::Value << background_.name().toStdString();
00784   std::string image_transport;
00785   if (node_->getParam(IMAGE_TRANSPORT_PARAM, image_transport))
00786   {
00787     out << YAML::Key << IMAGE_TRANSPORT_PARAM << YAML::Value << image_transport;
00788   }
00789 
00790   if (force_720p_)
00791   {
00792     out << YAML::Key << "force_720p" << YAML::Value << force_720p_;
00793   }
00794 
00795   if (force_480p_)
00796   {
00797     out << YAML::Key << "force_480p" << YAML::Value << force_480p_;
00798   }
00799 
00800   if (ui_.configs->count() > 0)
00801   {
00802     out << YAML::Key << "displays"<< YAML::Value << YAML::BeginSeq;
00803 
00804     for (int i = 0; i < ui_.configs->count(); i++)
00805     {
00806       out << YAML::BeginMap;
00807       out << YAML::Key << "type" << YAML::Value << plugins_[ui_.configs->item(i)]->Type();
00808       out << YAML::Key << "name" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Name().toStdString();
00809       out << YAML::Key << "config" << YAML::Value;
00810       out << YAML::BeginMap;
00811 
00812       out << YAML::Key << "visible" << YAML::Value << plugins_[ui_.configs->item(i)]->Visible();
00813       out << YAML::Key << "collapsed" << YAML::Value << (static_cast<ConfigItem*>(ui_.configs->itemWidget(ui_.configs->item(i))))->Collapsed();
00814 
00815       plugins_[ui_.configs->item(i)]->SaveConfig(out, config_path);
00816 
00817       out << YAML::EndMap;
00818       out << YAML::EndMap;
00819     }
00820 
00821     out << YAML::EndSeq;
00822   }
00823 
00824   out << YAML::EndMap;
00825 
00826   fout << out.c_str();
00827   fout.close();
00828 }
00829 
00830 void Mapviz::AutoSave()
00831 {
00832   QProcessEnvironment env = QProcessEnvironment::systemEnvironment();
00833   QString default_path = QDir::homePath();
00834 
00835   if (env.contains(ROS_WORKSPACE_VAR))
00836   {
00837     // Try to save our config in the ROS_WORKSPACE directory, but if we can't write
00838     // to that -- probably because it is read-only -- try to use the home directory
00839     // instead.
00840     QString ws_path = env.value(ROS_WORKSPACE_VAR, default_path);
00841     QString ws_file = ws_path + MAPVIZ_CONFIG_FILE;
00842     QFileInfo file_info(ws_file);
00843     QFileInfo dir_info(ws_path);
00844     if ((!file_info.exists() && dir_info.isWritable()) ||
00845         file_info.isWritable())
00846     {
00847       // Note that FileInfo::isWritable will return false if a file does not exist, so
00848       // we need to check both if the target file is writable and if the target dir is
00849       // writable if the file doesn't exist.
00850       default_path = ws_path;
00851     }
00852     else
00853     {
00854       ROS_WARN("Could not write config file to %s.  Trying home directory.",
00855                (ws_path + MAPVIZ_CONFIG_FILE).toStdString().c_str());
00856     }
00857   }
00858   default_path += MAPVIZ_CONFIG_FILE;
00859 
00860 
00861   Save(default_path.toStdString());
00862 }
00863 
00864 void Mapviz::OpenConfig()
00865 {
00866   QFileDialog dialog(this, "Select Config File");
00867   dialog.setFileMode(QFileDialog::ExistingFile);
00868   dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
00869 
00870   dialog.exec();
00871 
00872   if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
00873   {
00874     std::string path = dialog.selectedFiles().first().toStdString();
00875     Open(path);
00876   }
00877 }
00878 
00879 void Mapviz::SaveConfig()
00880 {
00881   QFileDialog dialog(this, "Save Config File");
00882   dialog.setFileMode(QFileDialog::AnyFile);
00883   dialog.setAcceptMode(QFileDialog::AcceptSave);
00884   dialog.setNameFilter(tr("Mapviz Config Files (*.mvc)"));
00885 
00886   dialog.exec();
00887 
00888   if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
00889   {
00890     std::string path = dialog.selectedFiles().first().toStdString();
00891 
00892     std::string title;
00893     size_t last_slash = path.find_last_of('/');
00894     if (last_slash != std::string::npos && last_slash != path.size() - 1)
00895     {
00896       title = path.substr(last_slash + 1) + " (" +
00897               path.substr(0, last_slash + 1) + ")";
00898     }
00899     else
00900     {
00901       title = path;
00902     }
00903 
00904     title += " - mapviz";
00905 
00906     setWindowTitle(QString::fromStdString(title));
00907 
00908     Save(path);
00909   }
00910 }
00911 
00912 void Mapviz::ClearHistory()
00913 {
00914   for (auto& plugin: plugins_)
00915   {
00916     plugin.second->ClearHistory();
00917   }
00918 }
00919 
00920 void Mapviz::SelectNewDisplay()
00921 {
00922   ROS_INFO("Select new display ...");
00923   QDialog dialog;
00924   Ui::pluginselect ui;
00925   ui.setupUi(&dialog);
00926 
00927   std::vector<std::string> plugins = loader_->getDeclaredClasses();
00928   std::map<std::string, std::string> plugin_types;
00929   for (size_t i = 0; i < plugins.size(); i++)
00930   {
00931     QString type(plugins[i].c_str());
00932     type = type.split('/').last();
00933     ui.displaylist->addItem(type);
00934     plugin_types[type.toStdString()] = plugins[i];
00935   }
00936   ui.displaylist->setCurrentRow(0);
00937 
00938   dialog.exec();
00939 
00940   if (dialog.result() == QDialog::Accepted)
00941   {
00942     std::string type_name = ui.displaylist->selectedItems().first()->text().toStdString();
00943     std::string type = plugin_types[type_name];
00944     std::string name = "new display";
00945     try
00946     {
00947       CreateNewDisplay(name, type, true, false);
00948     }
00949     catch (const pluginlib::LibraryLoadException& e)
00950     {
00951       std::stringstream message;
00952       message << "Unable to load " << type << "." << std::endl << "Check the ROS log for more details.";
00953       QMessageBox::warning(this, "Plugin failed to load", QString::fromStdString(message.str()));
00954       ROS_ERROR("%s", e.what());
00955     }
00956   }
00957 }
00958 
00959 bool Mapviz::AddDisplay(
00960       AddMapvizDisplay::Request& req,
00961       AddMapvizDisplay::Response& resp)
00962 {
00963   std::map<std::string, std::string> properties;
00964   for (auto& property: req.properties)
00965   {
00966     properties[property.key] = property.value;
00967   }
00968 
00969   YAML::Node config;
00970   if (!swri_yaml_util::LoadMap(properties, config))
00971   {
00972     ROS_ERROR("Failed to parse properties into YAML.");
00973     return false;
00974   }
00975 
00976   for (auto& display: plugins_)
00977   {
00978     MapvizPluginPtr plugin = display.second;
00979     if (!plugin)
00980     {
00981       ROS_ERROR("Invalid plugin ptr.");
00982       continue;
00983     }
00984     if (plugin->Name() == req.name && plugin->Type() ==req.type)
00985     {
00986       plugin->LoadConfig(config, "");
00987       plugin->SetVisible(req.visible);
00988 
00989       if (req.draw_order > 0)
00990       {
00991         display.first->setData(Qt::UserRole, QVariant(req.draw_order - 1.1));
00992         ui_.configs->sortItems();
00993 
00994         ReorderDisplays();
00995       }
00996       else if (req.draw_order < 0)
00997       {
00998         display.first->setData(Qt::UserRole, QVariant(ui_.configs->count() + req.draw_order + 0.1));
00999         ui_.configs->sortItems();
01000 
01001         ReorderDisplays();
01002       }
01003 
01004       resp.success = true;
01005 
01006       return true;
01007     }
01008   }
01009 
01010   try
01011   {
01012     MapvizPluginPtr plugin =
01013       CreateNewDisplay(req.name, req.type, req.visible, false, req.draw_order);
01014     plugin->LoadConfig(config, "");
01015     plugin->DrawIcon();
01016     resp.success = true;
01017   }
01018   catch (const pluginlib::LibraryLoadException& e)
01019   {
01020     ROS_ERROR("%s", e.what());
01021     resp.success = false;
01022     resp.message = "Failed to load display plug-in.";
01023   }
01024 
01025   return true;
01026 }
01027 
01028 void Mapviz::Hover(double x, double y, double scale)
01029 {
01030   if (ui_.statusbar->isVisible())
01031   {
01032     if (scale == 0)
01033     {
01034       xy_pos_label_->setVisible(false);
01035       lat_lon_pos_label_->setVisible(false);
01036       return;
01037     }
01038 
01039     int32_t precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / scale))));
01040 
01041     QString text = ui_.fixedframe->currentText();
01042     if (text.isEmpty() || text == "/")
01043     {
01044       text = "fixed";
01045     }
01046     text += ": ";
01047 
01048     std::ostringstream x_ss;
01049     x_ss << std::fixed << std::setprecision(precision);
01050     x_ss << x;
01051     text += x_ss.str().c_str();
01052 
01053     text += ", ";
01054 
01055     std::ostringstream y_ss;
01056     y_ss << std::fixed << std::setprecision(precision);
01057     y_ss << y;
01058     text += y_ss.str().c_str();
01059 
01060     xy_pos_label_->setText(text);
01061     xy_pos_label_->setVisible(true);
01062     xy_pos_label_->update();
01063 
01064     swri_transform_util::Transform transform;
01065     if (tf_manager_.SupportsTransform(
01066            swri_transform_util::_wgs84_frame,
01067            ui_.fixedframe->currentText().toStdString()) &&
01068         tf_manager_.GetTransform(
01069            swri_transform_util::_wgs84_frame,
01070            ui_.fixedframe->currentText().toStdString(),
01071            transform))
01072     {
01073       tf::Vector3 point(x, y, 0);
01074       point = transform * point;
01075 
01076       QString lat_lon_text = "lat/lon: ";
01077 
01078       double lat_scale = (1.0 / 111111.0) * scale;
01079       int32_t lat_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lat_scale))));
01080 
01081       std::ostringstream lat_ss;
01082       lat_ss << std::fixed << std::setprecision(lat_precision);
01083       lat_ss << point.y();
01084       lat_lon_text += lat_ss.str().c_str();
01085 
01086       lat_lon_text += ", ";
01087 
01088       double lon_scale = (1.0 / (111111.0 * std::cos(point.y() * swri_math_util::_deg_2_rad))) * scale;
01089       int32_t lon_precision = static_cast<int32_t>(std::ceil(std::max(0.0, std::log10(1.0 / lon_scale))));
01090 
01091       std::ostringstream lon_ss;
01092       lon_ss << std::fixed << std::setprecision(lon_precision);
01093       lon_ss << point.x();
01094       lat_lon_text += lon_ss.str().c_str();
01095 
01096       lat_lon_pos_label_->setText(lat_lon_text);
01097       lat_lon_pos_label_->setVisible(true);
01098       lat_lon_pos_label_->update();
01099     }
01100     else if (lat_lon_pos_label_->isVisible())
01101     {
01102       lat_lon_pos_label_->setVisible(false);
01103     }
01104   }
01105 }
01106 
01107 MapvizPluginPtr Mapviz::CreateNewDisplay(
01108     const std::string& name,
01109     const std::string& type,
01110     bool visible,
01111     bool collapsed,
01112     int draw_order)
01113 {
01114   ConfigItem* config_item = new ConfigItem();
01115 
01116   config_item->SetName(name.c_str());
01117 
01118   std::string real_type = type;
01119   if (real_type == "mapviz_plugins/mutlires_image")
01120   {
01121     // The "multires_image" plugin was originally accidentally named "mutlires_image".
01122     // Loading a mapviz config file that still has the old name would normally cause it
01123     // to crash, so this will check for and correct it.
01124     real_type = "mapviz_plugins/multires_image";
01125   }
01126 
01127 
01128   ROS_INFO("creating: %s", real_type.c_str());
01129   MapvizPluginPtr plugin = loader_->createInstance(real_type.c_str());
01130 
01131   // Setup configure widget
01132   config_item->SetWidget(plugin->GetConfigWidget(this));
01133   plugin->SetIcon(config_item->ui_.icon);
01134 
01135   plugin->Initialize(tf_, canvas_);
01136   plugin->SetType(real_type.c_str());
01137   plugin->SetName(name);
01138   plugin->SetNode(*node_);
01139   plugin->SetVisible(visible);
01140 
01141   if (draw_order == 0)
01142   {
01143     plugin->SetDrawOrder(ui_.configs->count());
01144   }
01145   else if (draw_order > 0)
01146   {
01147     plugin->SetDrawOrder(std::min(ui_.configs->count(), draw_order - 1));
01148   }
01149   else if (draw_order < 0)
01150   {
01151     plugin->SetDrawOrder(std::max(0, ui_.configs->count() + draw_order + 1));
01152   }
01153 
01154   QString pretty_type(real_type.c_str());
01155   pretty_type = pretty_type.split('/').last();
01156   config_item->SetType(pretty_type);
01157   QListWidgetItem* item = new PluginConfigListItem();
01158   config_item->SetListItem(item);
01159   item->setSizeHint(config_item->sizeHint());
01160   connect(config_item, SIGNAL(UpdateSizeHint()), this, SLOT(UpdateSizeHints()));
01161   connect(config_item, SIGNAL(ToggledDraw(QListWidgetItem*, bool)), this, SLOT(ToggleShowPlugin(QListWidgetItem*, bool)));
01162   connect(config_item, SIGNAL(RemoveRequest(QListWidgetItem*)), this, SLOT(RemoveDisplay(QListWidgetItem*)));
01163   connect(plugin.get(), SIGNAL(VisibleChanged(bool)), config_item, SLOT(ToggleDraw(bool)));
01164   connect(plugin.get(), SIGNAL(SizeChanged()), this, SLOT(UpdateSizeHints()));
01165 
01166   if (real_type == "mapviz_plugins/image")
01167   {
01168     // This is a little kludgey because we're relying on hard-coding a
01169     // plugin type here... feel free to suggest a better way.
01170     // If the default image transport has changed, we want to notify all of our
01171     // image plugins of it so that they will resubscribe appropriately.
01172     connect(this, SIGNAL(ImageTransportChanged()),
01173             plugin.get(), SLOT(Resubscribe()));
01174   }
01175 
01176   if (draw_order == 0)
01177   {
01178     ui_.configs->addItem(item);
01179   }
01180   else
01181   {
01182     ui_.configs->insertItem(plugin->DrawOrder(), item);
01183   }
01184 
01185   ui_.configs->setItemWidget(item, config_item);
01186   ui_.configs->UpdateIndices();
01187 
01188   // Add plugin to canvas
01189   plugin->SetTargetFrame(ui_.fixedframe->currentText().toStdString());
01190   plugin->SetUseLatestTransforms(ui_.uselatesttransforms->isChecked());
01191   plugins_[item] = plugin;
01192   canvas_->AddPlugin(plugin, -1);
01193 
01194   config_item->ToggleDraw(visible);
01195 
01196   if (collapsed)
01197     config_item->Hide();
01198 
01199   ReorderDisplays();
01200 
01201   return plugin;
01202 }
01203 
01204 void Mapviz::ToggleShowPlugin(QListWidgetItem* item, bool visible)
01205 {
01206   ROS_INFO("Toggle show plugin");
01207 
01208   if (plugins_.count(item) == 1)
01209   {
01210     plugins_[item]->SetVisible(visible);
01211   }
01212   canvas_->UpdateView();
01213 }
01214 
01215 void Mapviz::FixedFrameSelected(const QString& text)
01216 {
01217   if (!updating_frames_)
01218   {
01219     ROS_INFO("Fixed frame selected: %s", text.toStdString().c_str());
01220     if (canvas_ != NULL)
01221     {
01222       canvas_->SetFixedFrame(text.toStdString().c_str());
01223     }
01224   }
01225 }
01226 
01227 void Mapviz::TargetFrameSelected(const QString& text)
01228 {
01229   if (!updating_frames_)
01230   {
01231     ROS_INFO("Target frame selected: %s", text.toStdString().c_str());
01232 
01233     if (canvas_ != NULL)
01234     {
01235       canvas_->SetTargetFrame(text.toStdString().c_str());
01236     }
01237   }
01238 }
01239 
01240 void Mapviz::ToggleUseLatestTransforms(bool on)
01241 {
01242   canvas_->ToggleUseLatestTransforms(on);
01243 }
01244 
01245 void Mapviz::ToggleFixOrientation(bool on)
01246 {
01247   canvas_->ToggleFixOrientation(on);
01248 }
01249 
01250 void Mapviz::ToggleRotate90(bool on)
01251 {
01252   canvas_->ToggleRotate90(on);
01253 }
01254 
01255 void Mapviz::ToggleEnableAntialiasing(bool on)
01256 {
01257   canvas_->ToggleEnableAntialiasing(on);
01258 }
01259 
01260 void Mapviz::ToggleConfigPanel(bool on)
01261 {
01262   if (on)
01263   {
01264     ui_.configdock->show();
01265   }
01266   else
01267   {
01268     ui_.configdock->hide();
01269   }
01270 
01271   AdjustWindowSize();
01272 }
01273 
01274 void Mapviz::ToggleStatusBar(bool on)
01275 {
01276   ui_.statusbar->setVisible(on);
01277 
01278   AdjustWindowSize();
01279 }
01280 
01281 void Mapviz::ToggleCaptureTools(bool on)
01282 {
01283   if (on)
01284   {
01285     ui_.actionShow_Status_Bar->setChecked(true);
01286   }
01287 
01288   screenshot_button_->setVisible(on);
01289   rec_button_->setVisible(on);
01290   stop_button_->setVisible(on);
01291   spacer1_->setVisible(on);
01292   spacer2_->setVisible(on);
01293   spacer3_->setVisible(on);
01294 }
01295 
01296 void Mapviz::ToggleRecord(bool on)
01297 {
01298   stop_button_->setEnabled(true);
01299 
01300   if (on)
01301   {
01302     rec_button_->setIcon(QIcon(":/images/media-playback-pause.png"));
01303     rec_button_->setToolTip("Pause recording video of display canvas");
01304     if (!vid_writer_->isRecording())
01305     {
01306       // Lock the window size.
01307       AdjustWindowSize();
01308 
01309       canvas_->CaptureFrames(true);
01310 
01311       std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
01312       boost::replace_all(posix_time, ".", "_");
01313       std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".avi";
01314       boost::replace_all(filename, "~", getenv("HOME"));
01315 
01316 
01317       if (!vid_writer_->initializeWriter(filename, canvas_->width(), canvas_->height()))
01318       {
01319         ROS_ERROR("Failed to open video file for writing.");
01320         StopRecord();
01321         return;
01322       }
01323 
01324       ROS_INFO("Writing video to: %s", filename.c_str());
01325       ui_.statusbar->showMessage("Recording video to " + QString::fromStdString(filename));
01326 
01327       canvas_->updateGL();
01328     }
01329 
01330     record_timer_.start(1000.0 / 30.0);
01331   }
01332   else
01333   {
01334     rec_button_->setIcon(QIcon(":/images/media-record.png"));
01335     rec_button_->setToolTip("Continue recording video of display canvas");
01336     record_timer_.stop();
01337   }
01338 }
01339 
01340 void Mapviz::SetImageTransport(QAction* transport_action)
01341 {
01342   std::string transport = transport_action->text().toStdString();
01343   ROS_INFO("Setting %s to %s", IMAGE_TRANSPORT_PARAM.c_str(), transport.c_str());
01344   node_->setParam(IMAGE_TRANSPORT_PARAM, transport);
01345 
01346   Q_EMIT(ImageTransportChanged());
01347 }
01348 
01349 void Mapviz::UpdateImageTransportMenu()
01350 {
01351   QList<QAction*> actions = image_transport_menu_->actions();
01352 
01353   std::string current_transport;
01354   node_->param<std::string>(IMAGE_TRANSPORT_PARAM, current_transport, "raw");
01355   Q_FOREACH(QAction* action, actions)
01356   {
01357     if (action->text() == QString::fromStdString(current_transport))
01358     {
01359       action->setChecked(true);
01360       return;
01361     }
01362   }
01363 
01364   ROS_WARN("%s param was set to an unrecognized value: %s",
01365            IMAGE_TRANSPORT_PARAM.c_str(), current_transport.c_str());
01366 }
01367 
01368 void Mapviz::CaptureVideoFrame()
01369 {
01370   // We need to store the data inside a QImage in order to emit it as a
01371   // signal.
01372   // Note that the QImage here is set to "ARGB32", but it is actually BGRA.
01373   // Qt doesn't have a comparable BGR format, and the cv::VideoWriter this
01374   // is going to expects BGR format, but it'd be a waste for us to convert
01375   // to RGB and then back to BGR.
01376   QImage frame(canvas_->width(), canvas_->height(), QImage::Format_ARGB32);
01377   if (canvas_->CopyCaptureBuffer(frame.bits()))
01378   {
01379     Q_EMIT(FrameGrabbed(frame));
01380   }
01381   else
01382   {
01383     ROS_ERROR("Failed to get capture buffer");
01384   }
01385 }
01386 
01387 void Mapviz::Recenter()
01388 {
01389   canvas_->ResetLocation();
01390 }
01391 
01392 void Mapviz::StopRecord()
01393 {
01394   rec_button_->setChecked(false);
01395   stop_button_->setEnabled(false);
01396 
01397   record_timer_.stop();
01398   if (vid_writer_)
01399   {
01400     vid_writer_->stop();
01401   }
01402   canvas_->CaptureFrames(false);
01403 
01404   ui_.statusbar->showMessage(QString(""));
01405   rec_button_->setToolTip("Start recording video of display canvas");
01406 
01407   AdjustWindowSize();
01408 }
01409 
01410 void Mapviz::Screenshot()
01411 {
01412   canvas_->CaptureFrame(true);
01413 
01414   std::vector<uint8_t> frame;
01415   if (canvas_->CopyCaptureBuffer(frame))
01416   {
01417     cv::Mat image(canvas_->height(), canvas_->width(), CV_8UC4, &frame[0]);
01418     cv::Mat screenshot;
01419     cvtColor(image, screenshot, CV_BGRA2BGR);
01420 
01421     cv::flip(screenshot, screenshot, 0);
01422 
01423     std::string posix_time = boost::posix_time::to_iso_string(ros::WallTime::now().toBoost());
01424     boost::replace_all(posix_time, ".", "_");
01425     std::string filename = capture_directory_ + "/mapviz_" + posix_time + ".png";
01426     boost::replace_all(filename, "~", getenv("HOME"));
01427 
01428     ROS_INFO("Writing screenshot to: %s", filename.c_str());
01429     ui_.statusbar->showMessage("Saved image to " + QString::fromStdString(filename));
01430 
01431     cv::imwrite(filename, screenshot);
01432   }
01433   else
01434   {
01435     ROS_ERROR("Failed to take screenshot.");
01436   }
01437 }
01438 
01439 void Mapviz::UpdateSizeHints()
01440 {
01441   for (int i = 0; i < ui_.configs->count(); i++)
01442   {
01443     QListWidgetItem* item = ui_.configs->item(i);
01444     ConfigItem* widget = static_cast<ConfigItem*>(ui_.configs->itemWidget(item));
01445     if (widget) {
01446       // Make sure the ConfigItem in the QListWidgetItem we're getting really
01447       // exists; if this method is called before it's been initialized, it would
01448       // cause a crash.
01449       item->setSizeHint(widget->sizeHint());
01450     }
01451   }
01452 }
01453 
01454 void Mapviz::RemoveDisplay()
01455 {
01456   QListWidgetItem* item = ui_.configs->takeItem(ui_.configs->currentRow());
01457   RemoveDisplay(item);
01458 }
01459 
01460 void Mapviz::RemoveDisplay(QListWidgetItem* item)
01461 {
01462   ROS_INFO("Remove display ...");
01463 
01464   if (item)
01465   {
01466     canvas_->RemovePlugin(plugins_[item]);
01467     plugins_[item] = MapvizPluginPtr();
01468 
01469     delete item;
01470   }
01471 }
01472 
01473 void Mapviz::ClearDisplays()
01474 {
01475   while (ui_.configs->count() > 0)
01476   {
01477     ROS_INFO("Remove display ...");
01478 
01479     QListWidgetItem* item = ui_.configs->takeItem(0);
01480 
01481     canvas_->RemovePlugin(plugins_[item]);
01482     plugins_[item] = MapvizPluginPtr();
01483 
01484     delete item;
01485   }
01486 }
01487 
01488 void Mapviz::ReorderDisplays()
01489 {
01490   ROS_INFO("Reorder displays");
01491   for (int i = 0; i < ui_.configs->count(); i++)
01492   {
01493     plugins_[ui_.configs->item(i)]->SetDrawOrder(i);
01494   }
01495   canvas_->ReorderDisplays();
01496 }
01497 
01498 void Mapviz::SelectBackgroundColor(const QColor &color)
01499 {
01500   background_ = color;
01501   canvas_->SetBackground(background_);
01502 }
01503 
01504 void Mapviz::SetCaptureDirectory()
01505 {
01506   QFileDialog dialog(this, "Select Capture Directory");
01507   dialog.setFileMode(QFileDialog::DirectoryOnly);
01508 
01509   dialog.exec();
01510 
01511   if (dialog.result() == QDialog::Accepted && dialog.selectedFiles().count() == 1)
01512   {
01513     capture_directory_ = dialog.selectedFiles().first().toStdString();
01514   }
01515 }
01516 
01517 void Mapviz::HandleProfileTimer()
01518 {
01519   ROS_INFO("Mapviz Profiling Data");
01520   meas_spin_.printInfo("ROS SpinOnce()");
01521   for (auto& display: plugins_)
01522   {
01523     MapvizPluginPtr plugin = display.second;
01524     if (plugin)
01525     {
01526       plugin->PrintMeasurements();
01527     }
01528   }
01529 }
01530 }


mapviz
Author(s): Marc Alban
autogenerated on Thu Jun 6 2019 18:50:58