handler.cpp

Go to the documentation of this file.

#include <industrial_modbus_tcp/handler.hpp>

namespace industrial_modbus_tcp
{

Handler::Handler(const std::shared_ptr<ros::NodeHandle> nh,
                 const std::string name,
                 const std::shared_ptr<ros::Publisher> error_publisher,
                 const std::string server_ip,
                 const modbus_error_recovery_mode recovery,
                 const unsigned server_port):
  nh_(nh),
  error_publisher_(error_publisher),
  run_(false),
  name_(name),
  ctx_(modbus_new_tcp(server_ip.c_str(), server_port))
{
  if (ctx_ == nullptr)
    throw std::runtime_error("Unable to allocate libmodbus context:\n" + std::string(modbus_strerror(errno)));

  if (modbus_connect(ctx_) == -1)
  {
    // mobus_free is called in the destructor
    throw std::runtime_error("Modbus connection failed:\n" + std::string(modbus_strerror(errno)));
  }

  // Automatically reconnect in case of closed connection
  // No warning will be printed in case of auto-reconnect
  // The thread may hang while reconnecting!
  if (modbus_set_error_recovery(ctx_, recovery) != 0)
  {
    // modbus_close and mobus_free are called in the destructor
    throw std::runtime_error("Unable to set error recovery mode:\n" + std::string(modbus_strerror(errno)));
  }
}

Handler::~Handler()
{
  stop();
  modbus_close(ctx_);
  modbus_free(ctx_);
}

void Handler::stop()
{
  run_ = false;
  if (!threads_.empty())
  {
    for (std::unique_ptr<std::thread> &thread : threads_)
      thread->join(); // Stop thread after it's last execution
    for (std::unique_ptr<std::thread> &thread : threads_)
      thread.reset();
  }
}

void Handler::clearData()
{
  if (run_)
    stop();

  data_.clear();
}

void Handler::setData(const std::vector<std::shared_ptr<DataType>> &data)
{
  if (run_)
    stop();
  data_ = data;

  run_ = true;
  threads_.resize(data_.size());
  for (std::size_t i(0); i < threads_.size(); ++i)
    threads_[i].reset(new std::thread([this, i]
  { readData(i);}));
}

const std::vector<std::shared_ptr<DataType>> Handler::getData()
{
  return data_;
}

void Handler::readData(const std::size_t index)
{
  using std::chrono::system_clock;
  system_clock::time_point next_start_time(system_clock::now());

  while (nh_->ok() && run_)
  {
    next_start_time += data_.at(index)->poll_rate_;
    std::this_thread::sleep_until(next_start_time);

    uint16_t reg_values[data_.at(index)->number_of_registers_];

    int rc;
    // Lock mutex because a modbus context is NOT thread safe as advertised
    {
      std::lock_guard<std::mutex> lock(modbus_mutex_);
      rc = modbus_read_registers(
             ctx_,
             data_.at(index)->start_register_address_,
             data_.at(index)->number_of_registers_,
             reg_values);
    }
    if (rc == -1 || (std::size_t)rc != data_.at(index)->number_of_registers_)
    {
      std_msgs::String error;
      error.data = data_.at(index)->topic_name_ + ": " + modbus_strerror(errno);
      error_publisher_->publish(error);
      ROS_ERROR_STREAM(name_ << ": " << error.data);
      continue;
    }

    std::vector<uint16_t> regs;
    for (std::size_t i(0); i < data_.at(index)->number_of_registers_; ++i)
      regs.emplace_back(reg_values[i]);

    data_.at(index)->convertAndPublishData(regs);
  }
}

}