dataload_zcm.cpp

Go to the documentation of this file.

#include "dataload_zcm.h"

#include <QDebug>
#include <QFile>
#include <QInputDialog>
#include <QMainWindow>
#include <QMessageBox>
#include <QProgressDialog>
#include <QPushButton>
#include <QSettings>
#include <QTextStream>
#include <QWidget>
#include <QFileDialog>

#include <iostream>

#include <zcm/zcm-cpp.hpp>
#include <zcm/tools/Introspection.hpp>

using namespace std;

static bool verbose = false;

DataLoadZcm::DataLoadZcm()
{
  _dialog = new QDialog();
  _ui = new Ui::DialogZcm();
  _ui->setupUi(_dialog);

  _config_widget = new ConfigZCM("DataLoadZcm", _dialog);
  _ui->mainLayout->insertWidget(0, _config_widget, 1);

  _ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(false);

  _ui->listWidgetChannels->setSelectionMode(QAbstractItemView::ExtendedSelection);

  connect(_ui->listWidgetChannels, &QListWidget::itemSelectionChanged, this, [this]() {
      auto selected = _ui->listWidgetChannels->selectionModel()->selectedIndexes();
    bool box_enabled = selected.size() > 0;
    _ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(box_enabled);
  });
}

DataLoadZcm::~DataLoadZcm()
{
  delete _dialog;
}

const char* DataLoadZcm::name() const
{
  return "DataLoad Zcm";
}

const vector<const char*>& DataLoadZcm::compatibleFileExtensions() const
{
  static vector<const char*> extensions = { "zcmlog" };
  return extensions;
}

static int processInputLog(const string& logpath,
                           function<void(const zcm::LogEvent* evt)> processEvent)
{
    zcm::LogFile inlog(logpath, "r");
    if (!inlog.good()) {
        cerr << "Unable to open input zcm log: " << logpath << endl;
        return 1;
    }

    auto processLog = [&inlog](function<void(const zcm::LogEvent* evt)> processEvent) {
        const zcm::LogEvent* evt;
        off64_t offset;
        static int lastPrintPercent = 0;

        fseeko(inlog.getFilePtr(), 0, SEEK_END);
        off64_t logSize = ftello(inlog.getFilePtr());
        fseeko(inlog.getFilePtr(), 0, SEEK_SET);

        QProgressDialog progress_dialog;
        progress_dialog.setLabelText("Loading... please wait");
        progress_dialog.setWindowModality(Qt::ApplicationModal);
        progress_dialog.setRange(0, 100);
        progress_dialog.setAutoClose(true);
        progress_dialog.setAutoReset(true);
        progress_dialog.show();

        bool interrupted = false;

        while (1) {
            offset = ftello(inlog.getFilePtr());

            int percent = 100.0 * offset / (logSize == 0 ? 1 : logSize);
            if (percent != lastPrintPercent) {
                if (verbose) {
                    cout << "\r" << "Percent Complete: " << percent << flush;
                }
                lastPrintPercent = percent;

                progress_dialog.setValue(percent);
                if (progress_dialog.wasCanceled()) {
                  interrupted = true;
                  break;
                }
            }

            evt = inlog.readNextEvent();
            if (evt == nullptr) break;

            processEvent(evt);
        }
        if (verbose) {
            if (lastPrintPercent != 100 && !interrupted)
                cout << "\r" << "Percent Complete: 100" << flush;
            cout << endl;
        }

        if (interrupted) progress_dialog.cancel();
    };

    processLog(processEvent);

    inlog.close();

    return 0;
}

bool DataLoadZcm::refreshChannels(const string& filepath)
{
  _all_channels.clear();
  _all_channels_filepath = filepath;

  auto processEvent = [&](const zcm::LogEvent* evt){
    _all_channels.insert(evt->channel);
  };

  return processInputLog(filepath, processEvent) == 0;
}

bool DataLoadZcm::launchDialog(const string& filepath)
{
  QSettings settings;
  _dialog->restoreGeometry(settings.value("DataLoadZcm.geometry").toByteArray());

  _ui->listWidgetChannels->clear();
  for (auto& c : _all_channels) {
    auto chan = QString::fromStdString(c);
    _ui->listWidgetChannels->addItem(chan);
  }
  _ui->listWidgetChannels->sortItems();

  auto selected_channels = settings.value("DataLoadZcm.selected_channels").toStringList();
  for (int row = 0; row<_ui->listWidgetChannels->count(); row++) {
    auto item = _ui->listWidgetChannels->item(row);
    if (selected_channels.contains(item->text())) {
      item->setSelected(true);
    }
  }
  selected_channels.clear();

  int res = _dialog->exec();
  settings.setValue("DataLoadZcm.geometry", _dialog->saveGeometry());

  if (res == QDialog::Rejected) {
    return false;
  }

  _selected_channels.clear();

  QModelIndexList indexes = _ui->listWidgetChannels->selectionModel()->selectedRows();
  for (auto& i : indexes) {
    auto item = _ui->listWidgetChannels->item(i.row());
    _selected_channels.insert(item->text().toStdString());
    selected_channels.push_back(item->text());
  }

  settings.setValue("DataLoadZcm.selected_channels", selected_channels);

  return !indexes.empty();
}

template <typename T>
double toDouble(const void* data) {
  return static_cast<double>(*reinterpret_cast<const T*>(data));
}

struct ProcessUsr
{
  vector<pair<string, double>>& numerics;
  vector<pair<string, string>>& strings;
};
static void processData(const string& name, zcm_field_type_t type,
                        const void* data, void* usr)
{
  ProcessUsr* v = (ProcessUsr*)usr;
  switch (type) {
    case ZCM_FIELD_INT8_T: v->numerics.emplace_back(name, toDouble<int8_t>(data)); break;
    case ZCM_FIELD_INT16_T: v->numerics.emplace_back(name, toDouble<int16_t>(data)); break;
    case ZCM_FIELD_INT32_T: v->numerics.emplace_back(name, toDouble<int32_t>(data)); break;
    case ZCM_FIELD_INT64_T: v->numerics.emplace_back(name, toDouble<int64_t>(data)); break;
    case ZCM_FIELD_BYTE: v->numerics.emplace_back(name, toDouble<uint8_t>(data)); break;
    case ZCM_FIELD_FLOAT: v->numerics.emplace_back(name, toDouble<float>(data)); break;
    case ZCM_FIELD_DOUBLE: v->numerics.emplace_back(name, toDouble<double>(data)); break;
    case ZCM_FIELD_BOOLEAN: v->numerics.emplace_back(name, toDouble<bool>(data)); break;
    case ZCM_FIELD_STRING: v->strings.emplace_back(name, string((const char*)data)); break;
    case ZCM_FIELD_USER_TYPE: assert(false && "Should not be possble");
  }
};

bool DataLoadZcm::readDataFromFile(FileLoadInfo* info, PlotDataMapRef& plot_data)
{
  string filepath = info->filename.toStdString();

  if (info->plugin_config.hasChildNodes()) {
    xmlLoadState(info->plugin_config.firstChildElement());
  }
  if (filepath != _all_channels_filepath) {
    refreshChannels(filepath);
  }
  if (!launchDialog(filepath)) {
    return false;
  }

  zcm::TypeDb types(_config_widget->getLibraries().toStdString());
  if(!types.good())
  {
    QMessageBox::warning(nullptr, "Error", "Failed to load zcmtypes");
    return false;
  }

  vector<pair<string, double>> numerics;
  vector<pair<string, string>> strings;
  ProcessUsr usr = { numerics, strings };

  auto processEvent = [&](const zcm::LogEvent* evt){
    if (_selected_channels.find(evt->channel) == _selected_channels.end()) return;
    zcm::Introspection::processEncodedType(evt->channel,
                                           evt->data, evt->datalen,
                                           "/",
                                           types, processData, &usr);
    for (auto& n : usr.numerics) {
      auto itr = plot_data.numeric.find(n.first);
      if (itr == plot_data.numeric.end()) itr = plot_data.addNumeric(n.first);
      itr->second.pushBack({ (double)evt->timestamp / 1e6, n.second });
    }
    for (auto& s : usr.strings) {
      auto itr = plot_data.strings.find(s.first);
      if (itr == plot_data.strings.end()) itr = plot_data.addStringSeries(s.first);
      itr->second.pushBack({ (double)evt->timestamp / 1e6, s.second });
    }

    usr.numerics.clear();
    usr.strings.clear();
  };

  if (processInputLog(filepath, processEvent) != 0)
    return false;

  return true;
}

static QDomElement serialize(const unordered_set<string>& input,
                             QDomDocument& doc, const string& name)
{
  QDomElement ret = doc.createElement(QString::fromStdString(name));

  for (const auto& item : input) {
    QDomElement stringElement = doc.createElement("String");
    QDomText textNode = doc.createTextNode(QString::fromStdString(item));
    stringElement.appendChild(textNode);
    ret.appendChild(stringElement);
  }

  return ret;
}

static unordered_set<string> deserialize(const QDomElement& elt)
{
    unordered_set<string> ret;

    QDomNodeList childNodes = elt.childNodes();
    for (int i = 0; i < childNodes.size(); ++i) {
        QDomNode node = childNodes.item(i);
        if (node.isElement()) {
            QDomElement element = node.toElement();
            if (element.tagName() == "String") {
                QString stringValue = element.text();
                ret.insert(stringValue.toStdString());
            }
        }
    }

    return ret;
}


bool DataLoadZcm::xmlSaveState(QDomDocument& doc, QDomElement& parent_element) const
{
  parent_element.appendChild(serialize(_selected_channels, doc, "selected_channels"));
  auto all_channels = serialize(_all_channels, doc, "all_channels");
  all_channels.setAttribute("filepath", QString::fromStdString(_all_channels_filepath));
  parent_element.appendChild(all_channels);
  return true;
}

bool DataLoadZcm::xmlLoadState(const QDomElement& parent_element)
{
  QDomElement all_channels = parent_element.firstChildElement("all_channels");
  if (!all_channels.isNull()) {
    _all_channels = deserialize(all_channels);
    if (all_channels.hasAttribute("filepath")) {
      _all_channels_filepath = all_channels.attribute("filepath").toStdString();
    }
  }

  QDomElement selected_channels = parent_element.firstChildElement("selected_channels");
  if (!selected_channels.isNull()) {
    _selected_channels = deserialize(selected_channels);

    QStringList selected_channels;
    for (auto& c : _selected_channels)
      selected_channels.push_back(QString::fromStdString(c));

    QSettings settings;
    settings.setValue("DataLoadZcm.selected_channels", selected_channels);
  }

  return true;
}