node.cpp

Go to the documentation of this file.

#include <industrial_modbus_tcp/Configure.h>
#include <industrial_modbus_tcp/handler.hpp>
#include <ros/ros.h>

using namespace industrial_modbus_tcp;

std::shared_ptr<ros::NodeHandle> nh;
std::vector<std::shared_ptr<Handler>> modbus_handlers;
std::string base_topic;
std::shared_ptr<ros::Publisher> error_pub;

bool configureModbusCallback(industrial_modbus_tcp::ConfigureRequest &req,
                             industrial_modbus_tcp::ConfigureResponse &res)
{
  res.error.clear();

  if (req.handlers.empty())
  {
    res.error = "Zero handler provided";
    return true;
  }

  for (auto &handler : req.handlers)
  {
    if (handler.name.empty())
    {
      res.error = "Handler name cannot be empty";
      return true;
    }

    if (handler.ip_address.empty())
    {
      res.error = "Handler '" + handler.name + "' IP address cannot be empty";
      return true;
    }

    if (handler.d_bool.empty() && handler.d_float32.empty() && handler.d_float64.empty() &&
        handler.d_int16.empty() && handler.d_int32.empty() && handler.d_int64.empty() && handler.d_int8.empty() &&
        handler.d_uint16.empty() && handler.d_uint32.empty() && handler.d_uint64.empty() && handler.d_uint8.empty())
    {
      res.error = "Handler '" + handler.name + "' contains zero data to be fetched";
      return true;
    }
  }

  modbus_handlers.clear();
  for (auto &handler_config : req.handlers)
  {
    modbus_error_recovery_mode recovery_mode;
    switch (handler_config.modbus_error_recovery_mode)
    {
      case 0:
        recovery_mode = MODBUS_ERROR_RECOVERY_NONE;
        break;
      case 1:
        recovery_mode = MODBUS_ERROR_RECOVERY_LINK;
        break;
      case 2:
        recovery_mode = MODBUS_ERROR_RECOVERY_PROTOCOL;
        break;
      default:
        recovery_mode = (modbus_error_recovery_mode)(MODBUS_ERROR_RECOVERY_LINK | MODBUS_ERROR_RECOVERY_PROTOCOL);
        break;
    }

    std::shared_ptr<Handler> handler;
    try
    {
      if (handler_config.port == 0)
      {
        handler.reset(new Handler(nh,
                                  handler_config.name,
                                  error_pub,
                                  handler_config.ip_address,
                                  recovery_mode));
      }
      else
      {
        handler.reset(new Handler(nh,
                                  handler_config.name,
                                  error_pub,
                                  handler_config.ip_address,
                                  recovery_mode,
                                  handler_config.port));
      }
    }
    catch (std::runtime_error &e)
    {
      std_msgs::String error;
      error.data = "Handler '" + handler_config.name + "' error: " + e.what();
      error_pub->publish(error);
      res.error = error.data;
      return true;
    }

    // Add data into the handler
    std::vector<std::shared_ptr<DataType>> data;

    for (auto v : handler_config.d_bool)
    {
      std::shared_ptr<DataType> d = std::make_shared<BoolType>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<BoolType>(d)->mask_ = v.mask;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_int8)
    {
      std::shared_ptr<DataType> d = std::make_shared<Int8Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<Int8Type>(d)->bit_shift = v.bit_shift;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_uint8)
    {
      std::shared_ptr<DataType> d = std::make_shared<UInt8Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<UInt8Type>(d)->bit_shift = v.bit_shift;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_int16)
    {
      std::shared_ptr<DataType> d = std::make_shared<Int16Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_uint16)
    {
      std::shared_ptr<DataType> d = std::make_shared<UInt16Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_int32)
    {
      std::shared_ptr<DataType> d = std::make_shared<Int32Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<Int32Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_uint32)
    {
      std::shared_ptr<DataType> d = std::make_shared<UInt32Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<UInt32Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_int64)
    {
      std::shared_ptr<DataType> d = std::make_shared<Int64Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<Int64Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_uint64)
    {
      std::shared_ptr<DataType> d = std::make_shared<UInt64Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<UInt64Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_float32)
    {
      std::shared_ptr<DataType> d = std::make_shared<Float32Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<Float32Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    for (auto v : handler_config.d_float64)
    {
      std::shared_ptr<DataType> d = std::make_shared<Float64Type>(nh,
                                    v.reg.address,
                                    std::chrono::microseconds(v.reg.poll_rate_usec),
                                    v.reg.topic_name);
      std::dynamic_pointer_cast<Float64Type>(d)->big_endian = v.big_endian;
      data.emplace_back(d);
    }

    handler->setData(data);
    modbus_handlers.emplace_back(handler);
  }

  return true;
}

int main(int argc,
         char *argv[])
{
  ros::init(argc, argv, "modbus_tcp");
  nh = std::make_shared<ros::NodeHandle>("~");

  nh->param<std::string>("base_topic", base_topic, base_topic);
  error_pub = std::make_shared<ros::Publisher>();
  *error_pub = nh->advertise<std_msgs::String>(base_topic + "errors", 5);
  ros::ServiceServer service = nh->advertiseService(base_topic + "configure", configureModbusCallback);

  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::waitForShutdown();
  return 0;
}