mainwindow.cpp

Go to the documentation of this file.

#include <rqt_bag_exporter/mainwindow.hpp>

MainWindow::MainWindow(QWidget *parent) :
        QMainWindow(parent)
{
  qRegisterMetaType<QMessageBox::Icon>();
  parent = new QWidget;
  setCentralWidget(parent);
  QHBoxLayout *layout(new QHBoxLayout);
  parent->setLayout(layout);

  setWindowTitle("ROS bag exporter");

  QMenu *menu(new QMenu("File"));
  menuBar()->addMenu(menu);

  QAction *open_bag(new QAction("Open", menu));
  open_bag->setToolTip("Open a bag file");
  connect(open_bag, &QAction::triggered, this, &MainWindow::openBagFile);
  menu->addAction(open_bag);
  QAction *export_topics(new QAction("Export", menu));
  export_topics->setToolTip("Export topics to files");
  connect(export_topics, &QAction::triggered, this, &MainWindow::exportToDirectory);
  menu->addAction(export_topics);

  QVBoxLayout *left = new QVBoxLayout;
  left->addWidget(new QLabel("Topics to be exported"));
  topics_container_ = new QWidget;
  left->addWidget(topics_container_);
  left->addStretch(1);

  QVBoxLayout *right(new QVBoxLayout);
  right->addWidget(new QLabel("Start / end time"));
  start_end_time_ = new QWidget;
  time_begin_ = new QDoubleSpinBox;
  time_begin_->setEnabled(false);
  time_end_ = new QDoubleSpinBox;
  time_end_->setEnabled(false);
  QHBoxLayout *start_end_time(new QHBoxLayout);
  start_end_time->addWidget(time_begin_);
  start_end_time->addWidget(new QLabel(" sec"));
  start_end_time->addWidget(time_end_);
  start_end_time->addWidget(new QLabel(" sec"));
  start_end_time_->setLayout(start_end_time);
  right->addWidget(start_end_time_);
  right->addStretch(1);

  video_tab_ = new QTabWidget;
  QWidget* first_tab(new QWidget);
  QVBoxLayout *tab_information(new QVBoxLayout);
  QSpinBox* observed_fps(new QSpinBox);
  observed_fps->setEnabled(false);
  QHBoxLayout *video_fps(new QHBoxLayout);
  QSpinBox* fps(new QSpinBox);
  fps->setEnabled(false);
  video_fps->addWidget(new QLabel("Video FPS"));
  video_fps->addWidget(fps);
  tab_information->addWidget(new QLabel("Observed FPS"));
  tab_information->addWidget(observed_fps);
  tab_information->addLayout(video_fps);
  first_tab->setLayout(tab_information);
  video_tab_->addTab(first_tab, "Topic");
  right->addWidget(video_tab_);

  layout->addLayout(left);
  layout->addLayout(right);

  connect(time_begin_, SIGNAL(valueChanged(double)), this, SLOT(updateTimeEnd(double)));
  connect(time_end_, SIGNAL(valueChanged(double)), this, SLOT(updateTimeBegin(double)));
  connect(this, SIGNAL(displayMessageBox(const QString, const QString, const QString, const QMessageBox::Icon)), this,
          SLOT(displayMessageBoxHandler(const QString, const QString, const QString, const QMessageBox::Icon)));

  load();
  csv_ac_.reset(new ExportToCSVActionClient("export_csv", true));
  video_ac_.reset(new ExportToVideoActionClient("export_video", true));
}

MainWindow::~MainWindow()
{
}

void MainWindow::closeBag()
{
  ros::ServiceClient client = nh_.serviceClient<rqt_bag_exporter::CloseBag>("close_bag");
  rqt_bag_exporter::CloseBag srv;
  if (!client.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT displayMessageBox("Service error",
                             QString::fromStdString("Service server " + client.getService() + " does not exist."),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!client.call(srv))
  {
    Q_EMIT displayMessageBox("Error calling service",
                             QString::fromStdString("Failed to call service " + client.getService()),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!srv.response.error.empty())
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService() + " failed"),
                             QString::fromStdString(srv.response.error),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }
}

void MainWindow::openBagFile()
{
  const std::string file_name(QFileDialog::getOpenFileName(this, "Open bag file",
                                                           settings_->value("open_path").toString(),
                                                           "Bag files (*.bag)", Q_NULLPTR).toStdString());
  if (file_name.empty())
    return;

  std::string path_file(file_name);
  const int pos(path_file.find_last_of("/"));
  path_file.erase(pos, std::string::npos);
  settings_->setValue("open_path", QString::fromStdString(path_file));
  directory_ = path_file;
  ros::ServiceClient client = nh_.serviceClient<rqt_bag_exporter::OpenBag>("open_bag");
  if (!client.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT displayMessageBox("Service error",
                             QString::fromStdString("Service server " + client.getService() + " does not exist."),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  rqt_bag_exporter::OpenBag srv;
  srv.request.bag_file = file_name;
  if (!client.call(srv))
  {
    Q_EMIT displayMessageBox("Error calling service",
                             QString::fromStdString("Failed to call service " + client.getService()),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }
  if (!srv.response.error.empty())
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService() + " failed"),
                             QString::fromStdString(srv.response.error),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  listTopicsInBagFile();
  displayTopics();
  fillStartEndTime();
  video_tab_->clear();
  for (std::size_t i(0); i < topics_list_.topics_type.size(); ++i)
  {
    if (topics_list_.topics_type[i].find("sensor_msgs/CompressedImage") != std::string::npos)
      fillVideoTab(topics_list_.topics_name[i]);
  }
}

void MainWindow::listTopicsInBagFile()
{
  ros::ServiceClient client = nh_.serviceClient<rqt_bag_exporter::ListTopics>("list_topics");
  rqt_bag_exporter::ListTopics srv;
  if (!client.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT displayMessageBox("Service error",
                             QString::fromStdString("Service server " + client.getService() + " does not exist."),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!client.call(srv))
  {
    Q_EMIT displayMessageBox("Error calling service",
                             QString::fromStdString("Failed to call service " + client.getService()),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!srv.response.error.empty())
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService() + " failed"),
                             QString::fromStdString(srv.response.error),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (srv.response.topics_name.size() != srv.response.topics_type.size())
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService() + " failed"),
                             "Topics names and topics type are of different sizes",
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  topics_list_.topics_name.clear();
  topics_list_.topics_type.clear();
  for (std::size_t i(0); i < srv.response.topics_name.size(); ++i)
  {
    topics_list_.topics_name.push_back(srv.response.topics_name[i]);
    topics_list_.topics_type.push_back(srv.response.topics_type[i]);
  }
}

void MainWindow::displayTopics()
{
  QVBoxLayout* checkbox_layout(new QVBoxLayout);
  qDeleteAll(topics_container_->children());
  topics_container_->setLayout(checkbox_layout);
  for (std::size_t i(0); i < topics_list_.topics_name.size(); ++i)
  {
    const bool check_csv_type = isCsvWritableTopic(topics_list_.topics_type[i]);
    const bool check_video_type = isCompressedImageTopic(topics_list_.topics_type[i]);
    QHBoxLayout *sub_checkbox_layout(new QHBoxLayout);
    QCheckBox *checkbox = new QCheckBox(QString::fromStdString(topics_list_.topics_name[i]));

    if (!check_csv_type && !check_video_type)
      checkbox->setEnabled(false);

    checkbox->setObjectName(QString::fromStdString(topics_list_.topics_type[i]));
    sub_checkbox_layout->addWidget(checkbox);
    std::string topic_type("(" + topics_list_.topics_type[i] + ")");
    sub_checkbox_layout->addWidget(new QLabel(QString::fromStdString(topic_type)));
    checkbox_layout->addLayout(sub_checkbox_layout);
  }
}

bool MainWindow::isCompressedImageTopic(const std::string topic_type)
{
  if (topic_type.find("sensor_msgs/CompressedImage") != std::string::npos)
    return true;
  return false;
}

bool MainWindow::isCsvWritableTopic(const std::string topic_type)
{
  if (topic_type.find("std_msgs/Bool") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Duration") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Float32") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Float64") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Int16") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Int32") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Int64") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Int8") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/String") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/Time") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/UInt16") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/UInt32") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/UInt64") != std::string::npos)
    return true;

  if (topic_type.find("std_msgs/UInt8") != std::string::npos)
    return true;

  return false;
}

void MainWindow::fillStartEndTime()
{
  ros::ServiceClient client = nh_.serviceClient<rqt_bag_exporter::GetDuration>("get_duration");
  rqt_bag_exporter::GetDuration srv;
  if (!client.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT displayMessageBox("Service error",
                             QString::fromStdString("Service server " + client.getService() + " does not exist."),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }
  if (!client.call(srv))
  {
    Q_EMIT displayMessageBox("Error calling service",
                             QString::fromStdString("Failed to call service " + client.getService()),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!srv.response.error.empty())
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService() + " failed"),
                             QString::fromStdString(srv.response.error),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (srv.response.duration == 0)
  {
    Q_EMIT displayMessageBox(QString::fromStdString(client.getService()),
                             "Video duration is zero",
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  time_begin_->setEnabled(true);
  time_end_->setEnabled(true);
  time_begin_->setRange(0, srv.response.duration);
  time_end_->setRange(0, srv.response.duration);
  time_end_->setValue(srv.response.duration);
}

void MainWindow::fillVideoTab(const std::string topic_name)
{
  ros::ServiceClient client = nh_.serviceClient<rqt_bag_exporter::EstimateVideoFps>(
                                                                                    "estimate_video_fps");
  rqt_bag_exporter::EstimateVideoFps srv;
  srv.request.topic_name = topic_name;
  if (!client.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT displayMessageBox("Service error",
                             QString::fromStdString("Service server " + client.getService() + " does not exist."),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!client.call(srv))
  {
    Q_EMIT displayMessageBox("Error calling service",
                             QString::fromStdString("Failed to call service " + client.getService()),
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (srv.response.fps == 0)
  {
    Q_EMIT displayMessageBox("Error getting topics", "Fail to get back video fps of rosbag", "",
                             QMessageBox::Icon::Critical);
    return;
  }

  fps_ = srv.response.fps;
  QWidget* tab(new QWidget);
  QVBoxLayout *tab_information(new QVBoxLayout);
  QSpinBox* observed_fps(new QSpinBox);
  observed_fps->setEnabled(false);
  observed_fps->setValue(srv.response.fps);
  QHBoxLayout *video_fps(new QHBoxLayout);
  QSpinBox* fps(new QSpinBox);
  fps->setMaximum(999);
  fps->setValue(srv.response.fps);
  video_fps->addWidget(new QLabel("Video FPS"));
  video_fps->addWidget(fps);
  tab_information->addWidget(new QLabel("Observed FPS"));
  tab_information->addWidget(observed_fps);
  tab_information->addLayout(video_fps);
  tab->setLayout(tab_information);
  video_tab_->addTab(tab, QString::fromStdString(srv.request.topic_name));

  connect(fps, SIGNAL(valueChanged(int)), this, SLOT(updateFps(int)));
}

void MainWindow::updateFps(const int fps)
{
  fps_ = fps;
}

void MainWindow::updateTimeEnd(const double time_begin)
{
  if (time_begin >= time_end_->value())
    time_end_->setValue(time_begin + 0.01);
}

void MainWindow::updateTimeBegin(const double time_end)
{
  if (time_end <= time_begin_->value())
    time_begin_->setValue(time_end - 0.01);
}

void MainWindow::exportToDirectory()
{
  fillTopicsToExport();
  if (topics_to_export_.topics_name.empty())
  {
    Q_EMIT displayMessageBox("Zero topic to export", "Choose at least one topic to be exported", "",
                             QMessageBox::Icon::Warning);
    return;
  }

  directory_ =
      QFileDialog::getExistingDirectory(
                                        this,
                                        "Export directory",
                                        settings_->value("path_directory").toString(),
                                        QFileDialog::ShowDirsOnly).toStdString();
  if (directory_.empty())
    return;

  settings_->setValue("path_directory", QString::fromStdString(directory_));
  exportTopic();
}

void MainWindow::exportTopic()
{
  fillTopicsToExport();
  for (std::size_t i(0); i < topics_to_export_.topics_name.size(); ++i)
  {
    progress_dialog_ = std::make_shared<ProgressDialog>();
    bool check_csv_type = isCsvWritableTopic(topics_to_export_.topics_type[i]);
    bool check_video_type = isCompressedImageTopic(topics_to_export_.topics_type[i]);
    if (check_csv_type)
    {
      if (!csv_ac_->waitForServer(ros::Duration(1)))
      {
        Q_EMIT displayMessageBox("Error getting topics", "Action server does not exist.", "",
                                 QMessageBox::Icon::Critical);
        return;
      }
      rqt_bag_exporter::ExportToCSVGoal goal;
      rqt_bag_exporter::ExportToCSVResult result;
      goal.directory = directory_;
      goal.topic_name = topics_to_export_.topics_name[i];
      goal.start_time = time_begin_->value();
      goal.end_time = time_end_->value();
      csv_ac_->sendGoal(goal, boost::bind(&MainWindow::csvDoneCb, this, _1, _2), NULL,
                        boost::bind(&MainWindow::csvFeedbackCb, this, _1));
      if (!progress_dialog_->exec())
      {
        if (csv_ac_->getState().state_ == actionlib::SimpleClientGoalState::ACTIVE)
          csv_ac_->cancelAllGoals();
      }

      bool finished_before_timeout = csv_ac_->waitForResult();
      if (finished_before_timeout)
      {
        if (!result.error.empty())
        {
          Q_EMIT displayMessageBox("Error from action", QString::fromStdString(result.error), "",
                                   QMessageBox::Icon::Critical);
          return;
        }
        actionlib::SimpleClientGoalState state = csv_ac_->getState();
      }
      else
        Q_EMIT displayMessageBox("Error getting topics", "Action did not finish before the time out.", "",
                                 QMessageBox::Icon::Critical);
    }
    if (check_video_type)
    {
      if (!video_ac_->waitForServer(ros::Duration(1)))
      {
        Q_EMIT displayMessageBox("Error getting topics", "Action server does not exist.", "",
                                 QMessageBox::Icon::Critical);
        return;
      }
      rqt_bag_exporter::ExportToVideoGoal goal;
      rqt_bag_exporter::ExportToVideoResult result;
      goal.directory = directory_;
      goal.topic_name = topics_to_export_.topics_name[i];
      goal.start_time = time_begin_->value();
      goal.end_time = time_end_->value();
      goal.fps = fps_;
      video_ac_->sendGoal(goal, boost::bind(&MainWindow::videoDoneCb, this, _1, _2), NULL,
                          boost::bind(&MainWindow::videoFeedbackCb, this, _1));
      if (!progress_dialog_->exec())
      {
        if (video_ac_->getState().state_ == actionlib::SimpleClientGoalState::ACTIVE)
          video_ac_->cancelAllGoals();
      }
      bool finished_before_timeout = video_ac_->waitForResult();

      if (finished_before_timeout)
      {
        if (!result.error.empty())
        {
          Q_EMIT displayMessageBox("Error from action", QString::fromStdString(result.error), "",
                                   QMessageBox::Icon::Critical);
          return;
        }
        actionlib::SimpleClientGoalState state = video_ac_->getState();
      }
      else
        Q_EMIT displayMessageBox("Error getting topics", "Action did not finish before the time out.", "",
                                 QMessageBox::Icon::Critical);

    }
  }
}

void MainWindow::fillTopicsToExport()
{
  topics_to_export_.topics_name.clear();
  topics_to_export_.topics_type.clear();
  QList<QCheckBox *> all_check_box = topics_container_->findChildren<QCheckBox *>();
  for (int i(0); i < all_check_box.size(); ++i)
  {
    if (all_check_box[i]->isChecked())
    {
      topics_to_export_.topics_name.push_back(all_check_box[i]->text().toStdString());
      topics_to_export_.topics_type.push_back(all_check_box[i]->objectName().toStdString());
    }
  }
}

void MainWindow::csvDoneCb(const actionlib::SimpleClientGoalState &state,
                           const rqt_bag_exporter::ExportToCSVResultConstPtr &result)
{
  ros::Duration(0.1).sleep(); // Otherwise progress window bugs

  if (state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    if (result->error.empty())
    {
      if (progress_dialog_)
        Q_EMIT progress_dialog_->accept();
    }
  }

  if (state.state_ == actionlib::SimpleClientGoalState::ABORTED)
  {
    if (progress_dialog_)
      Q_EMIT progress_dialog_->reject();

    std::string error_msg("Action exportToCSV Aborted with error :" + result->error);
    Q_EMIT displayMessageBox("Error from action", QString::fromStdString(error_msg), "",
                             QMessageBox::Icon::Critical);
  }

  if (state.state_ == actionlib::SimpleClientGoalState::PREEMPTED)
  {
    if (progress_dialog_)
      Q_EMIT progress_dialog_->reject();
  }
}

void MainWindow::csvFeedbackCb(const rqt_bag_exporter::ExportToCSVFeedbackConstPtr &feedback)
{
  if (progress_dialog_ && progress_dialog_->isVisible())
    Q_EMIT progress_dialog_->drawProgress(feedback->progress_value, QString::fromStdString(feedback->progress_msg));
}

void MainWindow::videoDoneCb(const actionlib::SimpleClientGoalState &state,
                             const rqt_bag_exporter::ExportToVideoResultConstPtr &result)
{
  ros::Duration(0.1).sleep(); // Otherwise progress window bugs

  if (state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED)
  {
    if (result->error.empty())
    {
      if (progress_dialog_)
        Q_EMIT progress_dialog_->accept();
    }
  }

  if (state.state_ == actionlib::SimpleClientGoalState::ABORTED)
  {
    if (progress_dialog_)
      Q_EMIT progress_dialog_->reject();

    std::string error_msg("Action exportToVideo Aborted with error :" + result->error);
    Q_EMIT displayMessageBox("Error from action", QString::fromStdString(error_msg), "",
                             QMessageBox::Icon::Critical);

  }

  if (state.state_ == actionlib::SimpleClientGoalState::PREEMPTED)
  {
    if (progress_dialog_)
      Q_EMIT progress_dialog_->reject();
  }
}

void MainWindow::videoFeedbackCb(const rqt_bag_exporter::ExportToVideoFeedbackConstPtr &feedback)
{
  if (progress_dialog_ && progress_dialog_->isVisible())
    Q_EMIT progress_dialog_->drawProgress(feedback->progress_value, QString::fromStdString(feedback->progress_msg));
}

void MainWindow::closeEvent(QCloseEvent *event)
{
  closeBag();
  settings_ = new QSettings("Institut Maupertuis", "rqt_bag_exporter");
  settings_->setValue("geometry", saveGeometry());
  settings_->setValue("window_state", saveState());
  QMainWindow::closeEvent(event);
}

void MainWindow::load()
{
  settings_ = new QSettings("Institut Maupertuis", "rqt_bag_exporter");
  restoreGeometry(settings_->value("geometry").toByteArray());
  restoreState(settings_->value("window_state").toByteArray());
}

void MainWindow::displayMessageBoxHandler(const QString title,
                                          const QString message,
                                          const QString info_msg,
                                          const QMessageBox::Icon icon)
{
  const bool old(isEnabled());
  Q_EMIT setEnabled(false);
  QMessageBox msg_box;
  msg_box.setWindowTitle(title);
  msg_box.setText(message);
  msg_box.setInformativeText(info_msg);
  msg_box.setIcon(icon);
  msg_box.setStandardButtons(QMessageBox::Ok);
  msg_box.exec();
  Q_EMIT setEnabled(old);
}