00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <mapviz/mapviz.h>
00031
00032
00033 #include <cmath>
00034 #include <cstdlib>
00035 #include <algorithm>
00036 #include <fstream>
00037 #include <sstream>
00038
00039
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
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
00181
00182
00183
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
00231
00232
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
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
00281
00282
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
00838
00839
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
00848
00849
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
01122
01123
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
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
01169
01170
01171
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
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
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
01371
01372
01373
01374
01375
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
01447
01448
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 }