unittest_pilz_modbus_client.cpp

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/>.
 */

#include <gtest/gtest.h>

#include <chrono>
#include <memory>
#include <mutex>
#include <numeric>
#include <string>
#include <thread>
#include <vector>

#include <ros/ros.h>
#include <modbus/modbus.h>

#include <prbt_hardware_support/modbus_topic_definitions.h>
#include <prbt_hardware_support/ModbusMsgInStamped.h>
#include <prbt_hardware_support/pilz_modbus_client_mock.h>
#include <prbt_hardware_support/pilz_modbus_client.h>
#include <prbt_hardware_support/pilz_modbus_exceptions.h>
#include <prbt_hardware_support/pilz_modbus_client_exception.h>

#include <prbt_hardware_support/client_tests_common.h>

#include <pilz_testutils/async_test.h>

namespace pilz_modbus_client_test
{
using ::testing::_;
using ::testing::AnyNumber;
using ::testing::AtLeast;
using ::testing::Return;
using ::testing::Throw;
using ::testing::InSequence;
using ::testing::InvokeWithoutArgs;
using ::testing::Invoke;
using namespace prbt_hardware_support;

static constexpr unsigned int DEFAULT_MODBUS_PORT_TEST {502};
static constexpr unsigned int REGISTER_FIRST_IDX_TEST {512};
static constexpr unsigned int REGISTER_SIZE_TEST {2};
static constexpr unsigned int RESPONSE_TIMEOUT {10};

static constexpr double WAIT_FOR_START_TIMEOUT_S {3.0};
static constexpr double WAIT_SLEEPTIME_S {0.1};
static constexpr double WAIT_FOR_STOP_TIMEOUT_S {3.0};
static constexpr double WAIT_FOR_SERVICE_TIMEOUT_S {3.0};

class PilzModbusClientExecutor
{
public:
  PilzModbusClientExecutor(PilzModbusClient* client);

public:
  void start();
  void stop();

private:
  PilzModbusClient* client_;
  std::thread client_thread_;
};

PilzModbusClientExecutor::PilzModbusClientExecutor(PilzModbusClient* client)
  : client_(client)
{
}

void PilzModbusClientExecutor::start()
{
  client_thread_ = std::thread(&PilzModbusClient::run, client_);

  ros::Time start_waiting = ros::Time::now();
  while (!client_->isRunning())
  {
    ros::Duration(WAIT_SLEEPTIME_S).sleep();
    if (ros::Time::now() > start_waiting + ros::Duration(WAIT_FOR_START_TIMEOUT_S))
    {
      break;
    }
  }
}

void PilzModbusClientExecutor::stop()
{
  client_->terminate();
  ros::Time start_waiting = ros::Time::now();
  while (client_->isRunning())
  {
    ros::Duration(WAIT_SLEEPTIME_S).sleep();
    if (ros::Time::now() > start_waiting + ros::Duration(WAIT_FOR_STOP_TIMEOUT_S))
    {
      break;
    }
  }
  client_thread_.join();
}

class PilzModbusClientTests : public testing::Test, public testing::AsyncTest
{
public:
  virtual void SetUp();

protected:
  ros::Subscriber subscriber_;
  ros::AsyncSpinner spinner_{2};
  ros::NodeHandle nh_;

  MOCK_METHOD1(modbus_read_cb,  void(ModbusMsgInStamped msg));

};

void PilzModbusClientTests::SetUp()
{
  subscriber_ = nh_.subscribe<ModbusMsgInStamped>(prbt_hardware_support::TOPIC_MODBUS_READ, 1, &PilzModbusClientTests::modbus_read_cb, this);
  spinner_.start();
}

MATCHER_P(IsSuccessfullRead, vec, "") { return arg.holding_registers.data == vec; }
MATCHER(IsDisconnect, "") { return arg.disconnect.data; }


TEST_F(PilzModbusClientTests, testInitialization)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(true));

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_, registers, std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
}

TEST_F(PilzModbusClientTests, testInitializationWithRetry)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(2)
      .WillOnce(Return(false))
      .WillOnce(Return(true));

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST, REGISTER_SIZE_TEST, ros::Duration(0.1)));
}

TEST_F(PilzModbusClientTests, doubleInitialization)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(true));

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
  EXPECT_FALSE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
}

TEST_F(PilzModbusClientTests, failingInitialization)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(false));

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_FALSE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
}

TEST_F(PilzModbusClientTests, failingInitializationWithRetry)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(2)
      .WillOnce(Return(false))
      .WillOnce(Return(false));

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_FALSE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST, REGISTER_SIZE_TEST, ros::Duration(0.1)));
}

TEST_F(PilzModbusClientTests, properReadingAndDisconnect)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  {
    InSequence s;
    EXPECT_CALL(*mock, init(_,_))
        .Times(1)
        .WillOnce(Return(true));
    EXPECT_CALL(*mock, readHoldingRegister(_,_))
        .WillOnce(Return(std::vector<uint16_t>{1, 2}))
        .WillOnce(Return(std::vector<uint16_t>{1, 2}))
        .WillOnce(Return(std::vector<uint16_t>{3, 4}))
        .WillOnce(Throw(ModbusExceptionDisconnect("disconnect_message")));
  }
  {
    InSequence s;
    EXPECT_CALL(*this, modbus_read_cb(IsSuccessfullRead(std::vector<uint16_t>{1, 2})))
        .Times(2);
    EXPECT_CALL(*this, modbus_read_cb(IsSuccessfullRead(std::vector<uint16_t>{3, 4})))
        .Times(1);
    EXPECT_CALL(*this, modbus_read_cb(IsDisconnect()))
        .Times(1)
        .WillOnce(ACTION_OPEN_BARRIER_VOID("disconnected"));
  }

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client.init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
  EXPECT_NO_THROW(client.run());
  BARRIER("disconnected");
}

TEST_F(PilzModbusClientTests, runningWithoutInit)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_)).Times(0);
  EXPECT_CALL(*mock, readHoldingRegister(_,_)).Times(0);
  EXPECT_CALL(*this, modbus_read_cb(_)).Times(0);

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  PilzModbusClient client(nh_,registers,std::move(mock),
                          RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                          prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_THROW(client.run(), PilzModbusClientException);
}

TEST_F(PilzModbusClientTests, terminateRunningClient)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(true));
  ON_CALL(*mock, readHoldingRegister(_,_)).WillByDefault(Return(std::vector<uint16_t>{3, 4}));
  ON_CALL(*this, modbus_read_cb(IsSuccessfullRead(std::vector<uint16_t>{3,4})))
      .WillByDefault(Return());

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  auto client = std::make_shared< PilzModbusClient >(nh_,registers,std::move(mock),
                                                     RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                                                     prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client->init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));

  PilzModbusClientExecutor executor(client.get());
  executor.start();
  EXPECT_TRUE(client->isRunning());
  executor.stop();
  EXPECT_FALSE(client->isRunning());
}

void callbackDummy(const ModbusMsgInStampedConstPtr& /*msg*/) {}

TEST_F(PilzModbusClientTests, testTopicNameChange)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  const std::string topic_name {"test_topic_name"};

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  auto client = std::make_shared< PilzModbusClient >(nh_,registers,std::move(mock),
                                                     RESPONSE_TIMEOUT, topic_name, prbt_hardware_support::SERVICE_MODBUS_WRITE);
  // Wait for a moment to ensure the topic is "up and running"
  ros::Duration(WAIT_SLEEPTIME_S).sleep();
  ros::Subscriber sub = nh_.subscribe<ModbusMsgInStamped>(topic_name, 1, &callbackDummy);
  ASSERT_EQ(1u, sub.getNumPublishers());
}

TEST_F(PilzModbusClientTests, testSettingOfTimeOut)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  const unsigned int response_timeout {RESPONSE_TIMEOUT + 4};

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(true));

  EXPECT_CALL(*mock, setResponseTimeoutInMs(response_timeout)).Times(1);

  std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  auto client = std::make_shared< PilzModbusClient >(nh_,registers,std::move(mock),
                                                     response_timeout, prbt_hardware_support::TOPIC_MODBUS_READ,
                                                     prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(client->init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));
}

class HoldingRegisterIncreaser
{
public:
  HoldingRegisterIncreaser(unsigned int register_index = 0u)
    : register_index_(register_index)
  {}

public:
  RegCont readHoldingRegister(int /*addr*/, int nb)
  {
    RegCont vec( static_cast<RegCont::size_type>(nb) , 0u);
    vec.at(register_index_) = curr_count_;
    ++curr_count_;
    return vec;
  }

  void reset()
  {
    curr_count_ = 0;
  }

private:
  const unsigned int register_index_;
  uint16_t curr_count_ {0};
};

class RegisterBuffer
{
public:
  void add(ModbusMsgInStamped msg)
  {
    std::unique_lock<std::mutex> lk(m_);
    buffer_ = msg.holding_registers.data.at(0);
  }

  uint16_t get()
  {
    std::unique_lock<std::mutex> lk(m_);
    return buffer_;
  }

private:
  std::mutex m_;
  uint16_t buffer_;
};

TEST_F(PilzModbusClientTests, testSettingReadFrequency)
{
  std::vector<double> checked_frequencies {20.0, 40.0, 60.0};

  for(const auto& expected_read_frequency : checked_frequencies)
  {
    std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

    EXPECT_CALL(*mock, init(_,_))
        .Times(1)
        .WillOnce(Return(true));

    HoldingRegisterIncreaser fake_holding_register;
    ON_CALL(*mock, readHoldingRegister(_, _))
        .WillByDefault(Invoke(&fake_holding_register, &HoldingRegisterIncreaser::readHoldingRegister));


    RegisterBuffer buffer;
    ON_CALL(*this, modbus_read_cb(_))
        .WillByDefault(Invoke(&buffer, &RegisterBuffer::add));

    std::vector<unsigned short> registers(REGISTER_SIZE_TEST);
    std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

    auto client = std::make_shared< PilzModbusClient >(nh_,registers,std::move(mock),
                                                      RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                                                      prbt_hardware_support::SERVICE_MODBUS_WRITE,
                                                      expected_read_frequency);

    EXPECT_TRUE(client->init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST));

    PilzModbusClientExecutor executor(client.get());
    auto time_start = ros::Time::now();
    executor.start();

    ASSERT_TRUE(client->isRunning());

    // Time for measuring
    ros::Duration(1).sleep();

    auto final_value = buffer.get();
    auto time_stop = ros::Time::now();
    executor.stop();

    // Checks
    auto actual_frequency = (final_value - 1) / (time_stop - time_start).toSec();
    EXPECT_NEAR(expected_read_frequency, actual_frequency, 0.1 * expected_read_frequency);

    EXPECT_FALSE(client->isRunning());
  }


}

TEST_F(PilzModbusClientTests, testWritingOfHoldingRegister)
{
  std::unique_ptr<PilzModbusClientMock> mock(new PilzModbusClientMock());

  EXPECT_CALL(*mock, init(_,_))
      .Times(1)
      .WillOnce(Return(true));

  ON_CALL(*mock, readHoldingRegister(_, _))
      .WillByDefault(Return(std::vector<uint16_t>{1, 7}));

  // Data to be expected
  RegCont expected_write_reg {1, 5, 4};
  WriteModbusRegister reg_write_srv;
  reg_write_srv.request.holding_register_block.start_idx = 3;
  reg_write_srv.request.holding_register_block.values = expected_write_reg;

  RegCont expected_read_reg {14, 17};

  EXPECT_CALL( *mock, writeReadHoldingRegister(static_cast<int>(reg_write_srv.request.holding_register_block.start_idx),
                                               expected_write_reg,
                                               REGISTER_FIRST_IDX_TEST,
                                               static_cast<int>(expected_read_reg.size())) )
      .Times(1)
      .WillOnce(DoAll(ACTION_OPEN_BARRIER_VOID("writeHoldingRegister"), Return(expected_read_reg)));

  EXPECT_CALL(*this, modbus_read_cb(_))
      .Times(AnyNumber());

  EXPECT_CALL(*this, modbus_read_cb(IsSuccessfullRead(expected_read_reg)))
      .Times(AtLeast(1));

  std::vector<unsigned short> registers(expected_read_reg.size());
  std::iota(registers.begin(), registers.end(), REGISTER_FIRST_IDX_TEST);

  auto modbus_client = std::make_shared< PilzModbusClient >(nh_, registers, std::move(mock),
                                                            RESPONSE_TIMEOUT, prbt_hardware_support::TOPIC_MODBUS_READ,
                                                            prbt_hardware_support::SERVICE_MODBUS_WRITE);

  EXPECT_TRUE(modbus_client->init(LOCALHOST, DEFAULT_MODBUS_PORT_TEST)) << "Initialization of modbus client failed";

  ros::ServiceClient writer_client = nh_.serviceClient<WriteModbusRegister>(prbt_hardware_support::SERVICE_MODBUS_WRITE);
  ASSERT_TRUE(writer_client.waitForExistence(ros::Duration(WAIT_FOR_SERVICE_TIMEOUT_S))) << "Modbus write service was not advertised in time";

  PilzModbusClientExecutor executor(modbus_client.get());
  executor.start();
  EXPECT_TRUE(modbus_client->isRunning()) << "Modbus client not running";

  EXPECT_TRUE(writer_client.call(reg_write_srv)) << "Modbus write service failed";
  EXPECT_TRUE(reg_write_srv.response.success) << "Modbus write service could not write modbus register";

  BARRIER("writeHoldingRegister");

  executor.stop();
  EXPECT_FALSE(modbus_client->isRunning());
}


TEST_F(PilzModbusClientTests, testSplitIntoBlocksFcn){
  // lists with same elements will throw exception
  std::vector<unsigned short> in_throw = {1, 2, 1};
  ASSERT_THROW(PilzModbusClient::split_into_blocks(in_throw),
               PilzModbusClientException);

  // properly splitting into blocks
  std::vector<unsigned short> in_split = {1, 2, 4, 5};
  std::vector<std::vector<unsigned short>> out_split = PilzModbusClient::split_into_blocks(in_split);
  std::vector<std::vector<unsigned short>> expected_out_split = {{1, 2}, {4, 5}};
  EXPECT_EQ(out_split, expected_out_split);

  // not splitting already consecutive blocks
  std::vector<unsigned short> in_nosplit = {1, 2, 3};
  std::vector<std::vector<unsigned short>> out_nosplit = PilzModbusClient::split_into_blocks(in_nosplit);
  std::vector<std::vector<unsigned short>> expected_out_nosplit = {{1, 2, 3}};
  EXPECT_EQ(out_nosplit, expected_out_nosplit);
}
}  // namespace pilz_modbus_client_test

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "pilz_modbus_client_test");
  ros::NodeHandle nh;

  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}