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


mapviz
Author(s): Marc Alban
autogenerated on Thu Aug 24 2017 02:45:59