modbus_tcp.cpp

Go to the documentation of this file.

#include <industrial_modbus_tcp/panels/modbus_tcp.hpp>

namespace industrial_modbus_tcp
{

ModbusTCP::ModbusTCP(QWidget *parent) :
  rviz::Panel(parent)
{
  connect(this, &ModbusTCP::enable, this, &ModbusTCP::setEnabled);
  qRegisterMetaType<QMessageBox::Icon>();
  connect(this, &ModbusTCP::displayMessageBox, this, &ModbusTCP::displayMessageBoxHandler);

  setObjectName("ModbusTCP");
  setName(objectName());

  QVBoxLayout *layout(new QVBoxLayout);
  QWidget *scroll_widget = new QWidget;
  scroll_widget->setLayout(layout);
  QScrollArea *scroll_area = new QScrollArea;
  scroll_area->setWidget(scroll_widget);
  scroll_area->setWidgetResizable(true);
  scroll_area->setFrameShape(QFrame::NoFrame);
  QVBoxLayout *main_layout = new QVBoxLayout(this);
  main_layout->addWidget(scroll_area);

  stack_ = new QStackedWidget;
  QWidget *tab_base_topic(new QWidget);
  QVBoxLayout *tab_base_topic_layout(new QVBoxLayout(tab_base_topic));
  QWidget *tab_configure(new QWidget);
  QVBoxLayout *tab_configure_layout(new QVBoxLayout(tab_configure));
  stack_->addWidget(tab_base_topic);
  stack_->addWidget(tab_configure);
  layout->addWidget(stack_);

  base_topic_line_edit_ = new QLineEdit;
  base_topic_line_edit_->setText("industrial_modbus_tcp");
  QHBoxLayout *base_topic_layout(new QHBoxLayout);
  base_topic_layout->addWidget(new QLabel("Base topic: "));
  base_topic_layout->addWidget(base_topic_line_edit_);

  change_base_topic_ = new QPushButton("Ok");
  tab_base_topic_layout->addLayout(base_topic_layout);
  tab_base_topic_layout->addStretch(1);
  tab_base_topic_layout->addWidget(change_base_topic_);

  connect(this, &ModbusTCP::setStackIndex, this, [=](const int index)
  {
    stack_->setCurrentIndex(index);
  });

  connect(change_base_topic_, &QPushButton::clicked, this, [ = ]()
  {
    base_topic_ = base_topic_line_edit_->text().toStdString();
    configure_modbus_ = nh_.serviceClient<industrial_modbus_tcp::Configure>(base_topic_ + "/configure");
    errors_sub_ = nh_.subscribe(base_topic_ + "/errors", 100, &ModbusTCP::errorsCallback, this);
    QtConcurrent::run(this, &ModbusTCP::connectToService);
  });

  QPushButton *configure = new QPushButton("Configure");
  connect(configure, &QPushButton::clicked, this, &ModbusTCP::configureButtonHandler);

  errors_log_ = new QTableWidget(0, 2);
  {
    QStringList columns;
    columns.append("Time");
    columns.append("Error");
    errors_log_->setHorizontalHeaderLabels(columns);
    errors_log_->setColumnWidth(0, 130);
    errors_log_->setColumnWidth(1, 250);
  }
  connect(this, &ModbusTCP::newError, this, &ModbusTCP::newErrorHandler);

  tab_configure_layout->addWidget(configure);
  tab_configure_layout->addStretch(1);
  tab_configure_layout->addWidget(new QLabel("Errors:"));
  tab_configure_layout->addWidget(errors_log_);
}

ModbusTCP::~ModbusTCP()
{
}

void ModbusTCP::errorsCallback(const std_msgs::String::ConstPtr &msg)
{
  Q_EMIT newError(QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss"), QString::fromStdString(msg->data));
}

void ModbusTCP::newErrorHandler(const QString time, const QString msg)
{
  if (errors_log_->rowCount() >= 50)
    errors_log_->removeRow(0);

  errors_log_->setRowCount(errors_log_->rowCount() + 1);
  QLabel *time_label(new QLabel(time));
  QLabel *msg_label(new QLabel(msg));
  errors_log_->setCellWidget(errors_log_->rowCount() - 1, 0, time_label);
  errors_log_->setCellWidget(errors_log_->rowCount() - 1, 1, msg_label);
  errors_log_->setCurrentCell(errors_log_->rowCount() - 1, 0);
}

void ModbusTCP::configureButtonHandler()
{
  ModbusTCPConfigure *dialog = new ModbusTCPConfigure(configure_modbus_srv_.request);

  if (!dialog->exec())
    return;

  try
  {
    dialog->getConfigureRequest(configure_modbus_srv_.request);
  }
  catch (std::runtime_error &e)
  {
    Q_EMIT enable(true);
    Q_EMIT displayMessageBox("Error in handlers",
                             "The new configuration will not take effect because one of the handler contains an error:",
                             e.what(),
                             QMessageBox::Icon::Warning);
    return;
  }

  Q_EMIT configChanged();

  if (configure_modbus_srv_.request.handlers.empty())
    return;

  Q_EMIT enable(false);
  QtConcurrent::run(this, &ModbusTCP::configure);
}

void ModbusTCP::configure()
{
  if (configure_modbus_srv_.request.handlers.empty())
    return;

  if (!configure_modbus_.call(configure_modbus_srv_))
  {
    Q_EMIT enable(true);
    Q_EMIT displayMessageBox("Failed to call service",
                             "Could not call service to configure Modbus TCP",
                             "",
                             QMessageBox::Icon::Critical);
    return;
  }

  if (!configure_modbus_srv_.response.error.empty())
  {
    Q_EMIT enable(true);
    Q_EMIT displayMessageBox("Failed to call service",
                             "Service call to configure Modbus TCP failed:",
                             QString::fromStdString(configure_modbus_srv_.response.error),
                             QMessageBox::Icon::Warning);
    return;
  }

  Q_EMIT enable(true);
}

void ModbusTCP::load(const rviz::Config &config)
{
  rviz::Panel::load(config);

  bool tmp_bool;
  int tmp_int;
  QString tmp_str;

  unsigned i(0);
  while (1)
  {
    industrial_modbus_tcp::ModbusHandler handler;

    tmp_str.clear();
    if (!config.mapGetString("handler_" + QString::number(i) + "_ip_address", &tmp_str))
      break;
    if (tmp_str.isEmpty())
      break;
    handler.ip_address = tmp_str.toStdString();

    tmp_str.clear();
    if (!config.mapGetString("handler_" + QString::number(i) + "_name", &tmp_str))
      break;
    if (tmp_str.isEmpty())
      break;
    handler.name = tmp_str.toStdString();

    if (config.mapGetInt("handler_" + QString::number(i) + "_port", &tmp_int))
      handler.port = tmp_int;

    if (config.mapGetInt("handler_" + QString::number(i) + "_recovery_mode", &tmp_int))
      handler.modbus_error_recovery_mode = tmp_int;

    // Load data
    unsigned j(0);

    // Bool
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Bool d;
      if (!parseDataReg(config, i, j, "bool", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_bool_" + QString::number(j) + "_mask");
      if (!config.mapGetInt(token, &tmp_int))
        break;
      d.mask = tmp_int;

      handler.d_bool.emplace_back(d);
      ++j;
    }

    // Int8 and UInt8
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Int8 d;
      if (!parseDataReg(config, i, j, "int8", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_int8_" + QString::number(j) + "_bit_shift");
      if (!config.mapGetInt(token, &tmp_int))
        break;
      d.bit_shift = tmp_int;

      handler.d_int8.emplace_back(d);
      ++j;
    }
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::UInt8 d;
      if (!parseDataReg(config, i, j, "uint8", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_uint8_" + QString::number(j) + "_bit_shift");
      if (!config.mapGetInt(token, &tmp_int))
        break;
      d.bit_shift = tmp_int;

      handler.d_uint8.emplace_back(d);
      ++j;
    }

    // Int16 and UInt16
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Int16 d;
      if (!parseDataReg(config, i, j, "int16", d.reg))
        break;

      handler.d_int16.emplace_back(d);
      ++j;
    }
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::UInt16 d;
      if (!parseDataReg(config, i, j, "uint16", d.reg))
        break;

      handler.d_uint16.emplace_back(d);
      ++j;
    }

    // Int32 and UInt32
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Int32 d;
      if (!parseDataReg(config, i, j, "int32", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_int32_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_int32.emplace_back(d);
      ++j;
    }
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::UInt32 d;
      if (!parseDataReg(config, i, j, "uint32", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_uint32_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_uint32.emplace_back(d);
      ++j;
    }

    // Int64 and UInt64
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Int64 d;
      if (!parseDataReg(config, i, j, "int64", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_int64_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_int64.emplace_back(d);
      ++j;
    }
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::UInt64 d;
      if (!parseDataReg(config, i, j, "uint64", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_uint64_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_uint64.emplace_back(d);
      ++j;
    }

    // Float32 and Float64
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Float32 d;
      if (!parseDataReg(config, i, j, "float32", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_float32_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_float32.emplace_back(d);
      ++j;
    }
    j = 0;
    while (1)
    {
      industrial_modbus_tcp::Float64 d;
      if (!parseDataReg(config, i, j, "float64", d.reg))
        break;

      QString token("handler_" + QString::number(i) + "_float64_" + QString::number(j) + "_big_endian");
      if (!config.mapGetBool(token, &tmp_bool))
        break;
      d.big_endian = tmp_bool;

      handler.d_float64.emplace_back(d);
      ++j;
    }

    configure_modbus_srv_.request.handlers.push_back(handler);
    ++i;
  }

  if (config.mapGetString("base_topic", &tmp_str))
  {
    base_topic_line_edit_->setText(tmp_str);
    Q_EMIT change_base_topic_->clicked();
  }
}

bool ModbusTCP::parseDataReg(const rviz::Config &config,
                             const unsigned i,
                             const unsigned j,
                             const std::string name,
                             industrial_modbus_tcp::Register &r)
{
  int tmp_int;
  QString tmp_str;

  tmp_str.clear();
  QString token("handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_address");
  if (!config.mapGetInt(token, &tmp_int))
    return false;
  r.address = tmp_int;

  token = "handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_topic_name";
  if (!config.mapGetString(token, &tmp_str))
    return false;
  r.topic_name = tmp_str.toStdString();

  token = "handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_poll_rate_usec";
  if (!config.mapGetInt(token, &tmp_int))
    return false;
  r.poll_rate_usec = tmp_int;

  return true;
}

void ModbusTCP::saveDataReg(rviz::Config &config,
                            const unsigned i,
                            const unsigned j,
                            const std::string name,
                            const industrial_modbus_tcp::Register &r) const
{
  config.mapSetValue("handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_address",
                     r.address);
  config.mapSetValue("handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_topic_name",
                     QString::fromStdString(r.topic_name));
  config.mapSetValue("handler_" + QString::number(i) + "_" + QString::fromStdString(name) + "_" + QString::number(j) + "_poll_rate_usec",
                     r.poll_rate_usec);
}

void ModbusTCP::save(rviz::Config config) const
{
  rviz::Panel::save(config);
  if (!base_topic_.empty())
    config.mapSetValue("base_topic", QString::fromStdString(base_topic_));

  for (std::size_t i(0); i < configure_modbus_srv_.request.handlers.size(); ++i)
  {
    config.mapSetValue("handler_" + QString::number(i) +
                       "_ip_address",
                       QString::fromStdString(configure_modbus_srv_.request.handlers[i].ip_address));
    config.mapSetValue("handler_" + QString::number(i) +
                       "_name",
                       QString::fromStdString(configure_modbus_srv_.request.handlers[i].name));
    config.mapSetValue("handler_" + QString::number(i) +
                       "_port",
                       configure_modbus_srv_.request.handlers[i].port);
    config.mapSetValue("handler_" + QString::number(i) +
                       "_recovery_mode",
                       configure_modbus_srv_.request.handlers[i].modbus_error_recovery_mode);

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_bool.size(); ++j)
    {
      saveDataReg(config, i, j, "bool", configure_modbus_srv_.request.handlers.at(i).d_bool.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_bool_" + QString::number(j) + "_mask",
                         configure_modbus_srv_.request.handlers.at(i).d_bool.at(j).mask);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_int8.size(); ++j)
    {
      saveDataReg(config, i, j, "int8", configure_modbus_srv_.request.handlers.at(i).d_int8.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_int8_" + QString::number(j) + "_bit_shift",
                         configure_modbus_srv_.request.handlers.at(i).d_int8.at(j).bit_shift);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_uint8.size(); ++j)
    {
      saveDataReg(config, i, j, "uint8", configure_modbus_srv_.request.handlers.at(i).d_uint8.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_uint8_" + QString::number(j) + "_bit_shift",
                         configure_modbus_srv_.request.handlers.at(i).d_uint8.at(j).bit_shift);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_int16.size(); ++j)
    {
      saveDataReg(config, i, j, "int16", configure_modbus_srv_.request.handlers.at(i).d_int16.at(j).reg);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_uint16.size(); ++j)
    {
      saveDataReg(config, i, j, "uint16", configure_modbus_srv_.request.handlers.at(i).d_uint16.at(j).reg);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_int32.size(); ++j)
    {
      saveDataReg(config, i, j, "int32", configure_modbus_srv_.request.handlers.at(i).d_int32.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_int32_" + QString::number(j) + "_big_endian",
                         configure_modbus_srv_.request.handlers.at(i).d_int32.at(j).big_endian);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_uint32.size(); ++j)
    {
      saveDataReg(config, i, j, "uint32", configure_modbus_srv_.request.handlers.at(i).d_uint32.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_uint32_" + QString::number(j) + "_big_endian",
                         configure_modbus_srv_.request.handlers.at(i).d_uint32.at(j).big_endian);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_float32.size(); ++j)
    {
      saveDataReg(config, i, j, "float32", configure_modbus_srv_.request.handlers.at(i).d_float32.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_float32_" + QString::number(j) + "_big_endian",
                         configure_modbus_srv_.request.handlers.at(i).d_float32.at(j).big_endian);
    }

    for (std::size_t j(0); j < configure_modbus_srv_.request.handlers.at(i).d_float64.size(); ++j)
    {
      saveDataReg(config, i, j, "float64", configure_modbus_srv_.request.handlers.at(i).d_float64.at(j).reg);
      config.mapSetValue("handler_" + QString::number(i) + "_float64_" + QString::number(j) + "_big_endian",
                         configure_modbus_srv_.request.handlers.at(i).d_float64.at(j).big_endian);
    }
  }
}

void ModbusTCP::connectToService()
{
  Q_EMIT enable(false);
  if (!configure_modbus_.waitForExistence(ros::Duration(1)))
  {
    Q_EMIT setStackIndex(0);
    Q_EMIT enable(true);
    Q_EMIT displayMessageBox("Failed to connect to service",
                             QString::fromStdString(configure_modbus_.getService()),
                             "",
                             QMessageBox::Icon::Warning);
    return;
  }

  Q_EMIT setStackIndex(1);
  Q_EMIT enable(true);
  Q_EMIT configChanged();
  configure(); // Send configuration (if any)
}

void ModbusTCP::displayMessageBoxHandler(
  const QString title,
  const QString text,
  const QString info,
  const QMessageBox::Icon icon)
{
  const bool old_state(isEnabled());
  setEnabled(false);
  QMessageBox msg_box;
  msg_box.setWindowTitle(title);
  msg_box.setText(text);
  msg_box.setInformativeText(info);
  msg_box.setIcon(icon);
  msg_box.setStandardButtons(QMessageBox::Ok);
  msg_box.exec();
  setEnabled(old_state);
}

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(industrial_modbus_tcp::ModbusTCP, rviz::Panel)