pilz_modbus_client.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef PRBT_HARDWARE_SUPPORT_CLIENT_NODE_H
#define PRBT_HARDWARE_SUPPORT_CLIENT_NODE_H

#include <atomic>
#include <mutex>
#include <queue>

#include <boost/optional.hpp>

#include <ros/ros.h>
#include <std_msgs/UInt16MultiArray.h>

#include <prbt_hardware_support/modbus_client.h>
#include <prbt_hardware_support/register_container.h>
#include <prbt_hardware_support/WriteModbusRegister.h>

namespace prbt_hardware_support
{
class PilzModbusClient
{
  typedef std::unique_ptr<ModbusClient> ModbusClientUniquePtr;

public:
  PilzModbusClient(ros::NodeHandle& nh, const std::vector<unsigned short>& registers_to_read,
                   ModbusClientUniquePtr modbus_client, unsigned int response_timeout_ms,
                   const std::string& modbus_read_topic_name, const std::string& modbus_write_service_name,
                   double read_frequency_hz = DEFAULT_MODBUS_READ_FREQUENCY_HZ);

public:
  bool init(const char* ip, unsigned int port);

  bool init(const char* ip, unsigned int port, int retries, const ros::Duration& timeout_ms);

  void run();

  void terminate();

  bool isRunning();

  std::vector<std::vector<unsigned short>> static splitIntoBlocks(std::vector<unsigned short>& in);

private:
  void sendDisconnectMsg();

  bool modbus_write_service_cb(WriteModbusRegister::Request& req, WriteModbusRegister::Response& res);

private:
  enum State
  {
    not_initialized,
    initializing,
    initialized,
    running,
  };

  std::vector<unsigned short> registers_to_read_;

  const unsigned int RESPONSE_TIMEOUT_MS;
  const double READ_FREQUENCY_HZ;

private:
  static constexpr double DEFAULT_MODBUS_READ_FREQUENCY_HZ{ 500 };
  static constexpr int DEFAULT_QUEUE_SIZE_MODBUS{ 1 };

private:
  std::atomic<State> state_{ State::not_initialized };
  std::atomic_bool stop_run_{ false };
  ModbusClientUniquePtr modbus_client_;
  ros::Publisher modbus_read_pub_;
  ros::Subscriber modbus_write_sub_;
  ros::ServiceServer modbus_write_service_;

  std::mutex write_reg_blocks_mutex_;
  std::queue<ModbusRegisterBlock> write_reg_blocks_;
};

inline void PilzModbusClient::terminate()
{
  stop_run_ = true;
}

inline bool PilzModbusClient::isRunning()
{
  return state_.load() == State::running;
}

inline bool PilzModbusClient::modbus_write_service_cb(WriteModbusRegister::Request& req,
                                                      WriteModbusRegister::Response& res)
{
  std::lock_guard<std::mutex> lock(write_reg_blocks_mutex_);
  write_reg_blocks_.emplace(req.holding_register_block);
  res.success = true;
  return true;
}

}  // namespace prbt_hardware_support

#endif  // PRBT_HARDWARE_SUPPORT_CLIENT_NODE_H