datastream_zcm.cpp

Go to the documentation of this file.

#include "datastream_zcm.h"
 
#include <QTextStream>
#include <QFile>
#include <QMessageBox>
#include <QDebug>
 
#include <iomanip>
#include <iostream>
#include <mutex>
 
#include <QSettings>
#include <QFileDialog>
#include <QMessageBox>
 
using namespace std;
using namespace PJ;
 
template <typename T>
double toDouble(const void* data)
{
  return static_cast<double>(*reinterpret_cast<const T*>(data));
}
 
DataStreamZcm::DataStreamZcm() : _subs(nullptr), _running(false)
{
  _dialog = new QDialog;
  _ui = new Ui::DialogZcm;
  _ui->setupUi(_dialog);
 
  _config_widget = new ConfigZCM("DataStreamZcm", _dialog);
  _ui->mainLayout->insertWidget(5, _config_widget, 1);
}
 
DataStreamZcm::~DataStreamZcm()
{
  shutdown();
  delete _dialog;
}
 
const char* DataStreamZcm::name() const
{
  return "Zcm Streamer";
}
 
bool DataStreamZcm::start(QStringList*)
{
  if (_running)
  {
    return false;
  }
 
  // We create a Dialog to request the folder to populate zcm::TypeDb
  // and the string to pass to the subscriber.
 
  // Initialize the lineEdits in the ui with the previous value;
  QSettings settings;
  auto const subscribe_text = settings.value("DataStreamZcm::subscribe", ".*").toString();
  _ui->lineEditSubscribe->setText(subscribe_text);
 
  {
    auto transport = QString(getenv("ZCM_DEFAULT_URL"));
    transport = settings.value("DataStreamZcm::transport", transport).toString();
    _ui->lineEditTransport->setText(transport);
  }
 
  // start the dialog and check that OK was pressed
  _dialog->restoreGeometry(settings.value("DataStreamZcm::geometry").toByteArray());
  int res = _dialog->exec();
 
  settings.setValue("DataStreamZcm::geometry", _dialog->saveGeometry());
  if (res == QDialog::Rejected)
  {
    return false;
  }
 
  // save the current configuration for the next execution
  settings.setValue("DataStreamZcm::subscribe", _ui->lineEditSubscribe->text());
  settings.setValue("DataStreamZcm::transport", _ui->lineEditTransport->text());
 
  _transport = _ui->lineEditTransport->text();
 
  if (!_zcm)
  {
    try
    {
      _zcm.reset(new zcm::ZCM(_transport.toStdString()));
    }
    catch (std::exception& ex)
    {
      QMessageBox::warning(nullptr, "Error",
                           tr("Exception from zcm::ZCM() :\n%1").arg(ex.what()));
      return false;
    }
    if (!_zcm->good())
    {
      QMessageBox::warning(nullptr, "Error", "Failed to create zcm::ZCM()");
      _zcm.reset();
      return false;
    }
  }
 
  auto libraries = _config_widget->getLibraries();
 
  // reset the types if it is the first time or folder changed
  if (_types_library != libraries || !_types)
  {
    _types_library = libraries;
    _types.reset(new zcm::TypeDb(_types_library.toStdString()));
    if (!_types->good())
    {
      QMessageBox::warning(nullptr, "Error", "Failed to create zcm::TypeDb()");
      _types.reset();
      return false;
    }
  }
 
  if (_subscribe_string != _ui->lineEditSubscribe->text() || !_subs)
  {
    _subscribe_string = _ui->lineEditSubscribe->text();
    if (_subs)
    {
      _zcm->unsubscribe(_subs);
    }
    _subs =
        _zcm->subscribe(_subscribe_string.toStdString(), &DataStreamZcm::handler, this);
    if (!_subs)
    {
      QMessageBox::warning(nullptr, "Error", "Failed to subscribe");
      return false;
    }
  }
 
  _zcm->start();
  _running = true;
  return true;
}
 
void DataStreamZcm::shutdown()
{
  if (!_running)
  {
    return;
  }
  if (_subs)
  {
    _zcm->unsubscribe(_subs);
    _subs = nullptr;
  }
  _zcm->stop();
  _zcm.reset(nullptr);
  _running = false;
}
 
bool DataStreamZcm::isRunning() const
{
  return _running;
}
 
bool DataStreamZcm::xmlSaveState(QDomDocument& doc, QDomElement& parent_element) const
{
  // RRR (Bendes): Probably also transport string here
  QDomElement elem = doc.createElement("config");
  elem.setAttribute("folder", _types_library);
  elem.setAttribute("subscribe", _subscribe_string);
  elem.setAttribute("transport", _transport);
  parent_element.appendChild(elem);
  return true;
}
 
bool DataStreamZcm::xmlLoadState(const QDomElement& parent_element)
{
  QDomElement elem = parent_element.firstChildElement("config");
  if (!elem.isNull())
  {
    // RRR (Bendes): Probably also transport string here
    _types_library = elem.attribute("folder");
    _subscribe_string = elem.attribute("subscribe");
    _transport = elem.attribute("transport");
    QSettings settings;
    settings.setValue("DataStreamZcm::folder", _types_library);
    settings.setValue("DataStreamZcm::subscribe", _subscribe_string);
    settings.setValue("DataStreamZcm::transport", _transport);
  }
  return true;
}
 
void DataStreamZcm::processData(const string& name, zcm_field_type_t type,
                                const void* data, void* usr)
{
  DataStreamZcm* me = (DataStreamZcm*)usr;
  switch (type)
  {
    case ZCM_FIELD_INT8_T:
      me->_numerics.emplace_back(name, toDouble<int8_t>(data));
      break;
    case ZCM_FIELD_INT16_T:
      me->_numerics.emplace_back(name, toDouble<int16_t>(data));
      break;
    case ZCM_FIELD_INT32_T:
      me->_numerics.emplace_back(name, toDouble<int32_t>(data));
      break;
    case ZCM_FIELD_INT64_T:
      me->_numerics.emplace_back(name, toDouble<int64_t>(data));
      break;
    case ZCM_FIELD_BYTE:
      me->_numerics.emplace_back(name, toDouble<uint8_t>(data));
      break;
    case ZCM_FIELD_FLOAT:
      me->_numerics.emplace_back(name, toDouble<float>(data));
      break;
    case ZCM_FIELD_DOUBLE:
      me->_numerics.emplace_back(name, toDouble<double>(data));
      break;
    case ZCM_FIELD_BOOLEAN:
      me->_numerics.emplace_back(name, toDouble<bool>(data));
      break;
    case ZCM_FIELD_STRING:
      me->_strings.emplace_back(name, string((const char*)data));
      break;
    case ZCM_FIELD_USER_TYPE:
      assert(false && "Should not be possble");
  }
};
 
void DataStreamZcm::handler(const zcm::ReceiveBuffer* rbuf, const string& channel)
{
  zcm::Introspection::processEncodedType(channel, rbuf->data, rbuf->data_size, "/",
                                         *_types.get(), processData, this);
  {
    std::lock_guard<std::mutex> lock(mutex());
 
    for (auto& n : _numerics)
    {
      auto itr = dataMap().numeric.find(n.first);
      if (itr == dataMap().numeric.end())
      {
        itr = dataMap().addNumeric(n.first);
      }
      itr->second.pushBack({ double(rbuf->recv_utime) / 1e6, n.second });
    }
    for (auto& s : _strings)
    {
      auto itr = dataMap().strings.find(s.first);
      if (itr == dataMap().strings.end())
      {
        itr = dataMap().addStringSeries(s.first);
      }
      itr->second.pushBack({ double(rbuf->recv_utime) / 1e6, s.second });
    }
  }
 
  emit dataReceived();
 
  _numerics.clear();
  _strings.clear();
}
 
void DataStreamZcm::on_pushButtonUrl_clicked()
{
  QString url = getenv("ZCM_DEFAULT_URL");
  if (url.isEmpty())
  {
    QMessageBox::warning(nullptr, "Error",
                         "Environment variable ZCM_DEFAULT_URL not set");
  }
  else
  {
    _ui->lineEditTransport->setText(url);
  }
}