unittest_modbus_adapter_brake_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <gtest/gtest.h>
19 #include <gmock/gmock.h>
20 #include <functional>
21 #include <memory>
22 
23 #include <pilz_msgs/IsBrakeTestRequired.h>
24 #include <pilz_msgs/IsBrakeTestRequiredResult.h>
25 
35 
36 #include <pilz_testutils/async_test.h>
37 
38 namespace prbt_hardware_support
39 {
40 using namespace modbus_api::v3;
41 
44 
47 
48 using std::placeholders::_1;
49 using std::placeholders::_2;
50 
51 using ::testing::_;
52 using ::testing::DoAll;
53 using ::testing::Return;
54 using ::testing::SetArgReferee;
55 
57 {
58 public:
59  MOCK_METHOD2(modbsWriteRegisterFunc, bool(const uint16_t&, const RegCont&));
60 };
61 
62 static ModbusMsgInStampedPtr createDefaultBrakeTestModbusMsg(
63  const uint16_t brake_test_required_value, const unsigned int modbus_api_version = MODBUS_API_VERSION_REQUIRED,
64  const uint32_t brake_test_required_index = TEST_API_SPEC.getRegisterDefinition(modbus_api_spec::BRAKETEST_REQUEST))
65 {
66  const uint32_t first_index_to_read{ TEST_API_SPEC.getRegisterDefinition(modbus_api_spec::VERSION) };
67  const uint32_t last_index_to_read{ brake_test_required_index };
68  static int msg_time_counter{ 1 };
69  RegCont tab_reg(last_index_to_read - first_index_to_read + 1);
70  tab_reg[0] = static_cast<uint16_t>(modbus_api_version);
71  tab_reg[last_index_to_read - first_index_to_read] = brake_test_required_value;
72  ModbusMsgInStampedPtr msg{ ModbusMsgInBuilder::createDefaultModbusMsgIn(first_index_to_read, tab_reg) };
73  msg->header.stamp = ros::Time(msg_time_counter++);
74  return msg;
75 }
76 
81 TEST(ModbusAdapterBrakeTestTest, testModbusMsgBrakeTestWrapperExceptionDtor)
82 {
83  std::shared_ptr<ModbusMsgBrakeTestWrapperException> msg_wrapper{ new ModbusMsgBrakeTestWrapperException("Test msg") };
84 }
85 
90 TEST(ModbusAdapterBrakeTestTest, testModbusMsgBrakeTestWrapperDtor)
91 {
92  {
93  std::shared_ptr<ModbusMsgBrakeTestWrapper> msg_wrapper{ new ModbusMsgBrakeTestWrapper(
95  }
96 }
97 
109 TEST(ModbusAdapterBrakeTestTest, testNoMessageReceived)
110 {
111  ModbusMock mock;
112  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
114 
115  pilz_msgs::IsBrakeTestRequired srv;
116  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
117  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
118 }
119 
132 TEST(ModbusAdapterBrakeTestTest, testBrakeTestRequired)
133 {
134  ModbusMock mock;
135  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
137 
139 
140  pilz_msgs::IsBrakeTestRequired srv;
141  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
142  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::REQUIRED, srv.response.result.value);
143 }
144 
157 TEST(ModbusAdapterBrakeTestTest, testBrakeTestNotRequired)
158 {
159  ModbusMock mock;
160  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
162 
164 
165  pilz_msgs::IsBrakeTestRequired srv;
166  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
167  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::NOT_REQUIRED, srv.response.result.value);
168 }
169 
182 TEST(ModbusAdapterBrakeTestTest, testDisconnect)
183 {
184  constexpr uint32_t offset{ 0 };
185  const RegCont holding_register;
186  ModbusMsgInStampedPtr msg{ ModbusMsgInBuilder::createDefaultModbusMsgIn(offset, holding_register) };
187  msg->disconnect.data = true;
188 
189  ModbusMock mock;
190  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
192 
193  brake_test_adapter.modbusMsgCallback(msg);
194 
195  pilz_msgs::IsBrakeTestRequired srv;
196  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
197  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
198 }
199 
212 TEST(ModbusAdapterBrakeTestTest, testModbusIncorrectApiVersion)
213 {
214  ModbusMock mock;
215  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
217 
219 
220  pilz_msgs::IsBrakeTestRequired srv;
221  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
222  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
223 }
224 
236 TEST(ModbusAdapterBrakeTestTest, testModbusWithoutApiVersion)
237 {
241  msg->holding_registers.data.clear();
242 
243  ModbusMock mock;
244  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
246 
247  brake_test_adapter.modbusMsgCallback(msg);
248 
249  pilz_msgs::IsBrakeTestRequired srv;
250  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
251  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
252 }
253 
266 TEST(ModbusAdapterBrakeTestTest, testBrakeTestRequiredRegisterMissing)
267 {
268  ModbusMock mock;
269  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
271 
275 
276  pilz_msgs::IsBrakeTestRequired srv;
277  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
278  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
279 }
280 
292 TEST(ModbusAdapterBrakeTestTest, testBrakeTestRequiredRegisterUndefinedValue)
293 {
294  ModbusMock mock;
295  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
297 
298  brake_test_adapter.modbusMsgCallback(createDefaultBrakeTestModbusMsg(555 /* some arbitrary value */));
299 
300  pilz_msgs::IsBrakeTestRequired srv;
301  EXPECT_TRUE(brake_test_adapter.isBrakeTestRequired(srv.request, srv.response));
302  EXPECT_EQ(pilz_msgs::IsBrakeTestRequiredResult::UNKNOWN, srv.response.result.value);
303 }
304 
309 TEST(ModbusAdapterBrakeTestTest, testModbusApiSpecExceptionDtor)
310 {
311  std::shared_ptr<ModbusAdapterBrakeTestException> ex{ new ModbusAdapterBrakeTestException("Test msg") };
312 }
313 
330 TEST(ModbusAdapterBrakeTestTest, testBrakeTestTriggeringWrongApiDef)
331 {
332  /**********
333  * Step 1 *
334  **********/
335  {
336  ModbusApiSpec api_write_spec{ { modbus_api_spec::BRAKETEST_RESULT, 78 } };
337  ModbusMock mock;
338  ASSERT_THROW(ModbusAdapterBrakeTest bte_no_perf(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
339  TEST_API_SPEC, api_write_spec),
341  }
342 
343  /**********
344  * Step 2 *
345  **********/
346  {
347  ModbusApiSpec api_write_spec{ { modbus_api_spec::BRAKETEST_PERFORMED, 77 } };
348  ModbusMock mock;
349  ASSERT_THROW(ModbusAdapterBrakeTest bte_no_res(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
350  TEST_API_SPEC, api_write_spec),
352  }
353 
354  /**********
355  * Step 3 *
356  **********/
357  {
358  ModbusApiSpec api_write_spec{ { modbus_api_spec::BRAKETEST_PERFORMED, 100 },
360  ModbusMock mock;
361  ASSERT_NO_THROW(ModbusAdapterBrakeTest bte_one_apart(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
362  TEST_API_SPEC, api_write_spec));
363  }
364 
365  /**********
366  * Step 4 *
367  **********/
368  {
369  ModbusApiSpec api_write_spec{ { modbus_api_spec::BRAKETEST_PERFORMED, 100 },
371  ModbusMock mock;
372  ASSERT_THROW(ModbusAdapterBrakeTest bte_two_apart(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
373  TEST_API_SPEC, api_write_spec),
375  }
376 }
377 
382 TEST(ModbusAdapterBrakeTestTest, testMissingModbusWriteFunc)
383 {
384  ModbusMock mock;
385  ModbusAdapterBrakeTest brake_test_adapter(nullptr, TEST_API_SPEC, TEST_API_WRITE_SPEC);
386 
387  SendBrakeTestResult srv;
388  EXPECT_TRUE(brake_test_adapter.sendBrakeTestResult(srv.request, srv.response));
389  EXPECT_FALSE(srv.response.success);
390 }
391 
396 TEST(ModbusAdapterBrakeTestTest, testFailingModbusWriteFunc)
397 {
398  ModbusMock mock;
399  EXPECT_CALL(mock, modbsWriteRegisterFunc(_, _)).Times(1).WillOnce(Return(false));
400  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
402 
403  SendBrakeTestResult srv;
404  EXPECT_TRUE(brake_test_adapter.sendBrakeTestResult(srv.request, srv.response));
405  EXPECT_FALSE(srv.response.success);
406 }
407 
412 TEST(ModbusAdapterBrakeTestTest, testSecondTimeFailingModbusWriteFunc)
413 {
414  ModbusMock mock;
415  EXPECT_CALL(mock, modbsWriteRegisterFunc(_, _)).Times(2).WillOnce(Return(true)).WillOnce(Return(false));
416  ModbusAdapterBrakeTest brake_test_adapter(std::bind(&ModbusMock::modbsWriteRegisterFunc, &mock, _1, _2),
418 
419  SendBrakeTestResult srv;
420  EXPECT_TRUE(brake_test_adapter.sendBrakeTestResult(srv.request, srv.response));
421  EXPECT_FALSE(srv.response.success);
422 }
423 
425 {
426 public:
427  MOCK_METHOD1(call, bool(WriteModbusRegister& srv));
428  MOCK_METHOD0(getService, std::string());
429 };
430 
434 TEST(ModbusAdapterBrakeTestTest, testWriteModbusRegisterCallResponseFalse)
435 {
436  WriteModbusRegister res_exp;
437  res_exp.response.success = false;
438 
439  ServiceMock mock;
440  EXPECT_CALL(mock, getService()).WillRepeatedly(Return("TestServiceName"));
441  EXPECT_CALL(mock, call(_)).Times(1).WillOnce(DoAll(SetArgReferee<0>(res_exp), Return(true)));
442 
443  EXPECT_FALSE(writeModbusRegisterCall<ServiceMock>(mock, 0, {}));
444 }
445 
449 TEST(ModbusAdapterBrakeTestTest, testWriteModbusRegisterCallFailure)
450 {
451  WriteModbusRegister res_exp;
452  res_exp.response.success = false;
453 
454  ServiceMock mock;
455  EXPECT_CALL(mock, getService()).WillRepeatedly(Return("TestServiceName"));
456  EXPECT_CALL(mock, call(_)).Times(1).WillOnce(Return(false));
457 
458  EXPECT_FALSE(writeModbusRegisterCall<ServiceMock>(mock, 0, {}));
459 }
460 
461 } // namespace prbt_hardware_support
462 
463 int main(int argc, char* argv[])
464 {
465  testing::InitGoogleMock(&argc, argv);
466  return RUN_ALL_TESTS();
467 }
msg
static const std::string BRAKETEST_PERFORMED
static const std::string BRAKETEST_REQUEST
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
unsigned short getRegisterDefinition(const std::string &key) const
static const std::string BRAKETEST_RESULT
static const ModbusApiSpec TEST_API_WRITE_SPEC
static constexpr uint16_t REGISTER_VALUE_BRAKETEST_NOT_REQUIRED
bool call(const std::string &service_name, MReq &req, MRes &res)
int main(int argc, char *argv[])
Expection thrown upon construction of ModbusMsgBrakeTestWrapper of the message does not contain the r...
static const ModbusApiSpec TEST_API_SPEC
Exception thrown by the ModbusAdapterBrakeTest class.
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
bool sendBrakeTestResult(SendBrakeTestResult::Request &req, SendBrakeTestResult::Response &res)
Sends the brake test result data to the modbus client.
static ModbusMsgInStampedPtr createDefaultModbusMsgIn(const std_msgs::MultiArrayLayout::_data_offset_type &offset, const RegCont &holding_register)
Creates a standard ModbusMsgIn which contains default values for all essential elements of the messag...
static constexpr uint16_t REGISTER_VALUE_BRAKETEST_REQUIRED
bool isBrakeTestRequired(pilz_msgs::IsBrakeTestRequired::Request &, pilz_msgs::IsBrakeTestRequired::Response &res)
Stores the brake test required flag and initializes the brake test service, the first time the functi...
static ModbusMsgInStampedPtr createDefaultBrakeTestModbusMsg(const uint16_t brake_test_required_value, const unsigned int modbus_api_version=MODBUS_API_VERSION_REQUIRED, const uint32_t brake_test_required_index=TEST_API_SPEC.getRegisterDefinition(modbus_api_spec::BRAKETEST_REQUEST))
Wrapper class to add semantic to a raw ModbusMsgInStamped.
void modbusMsgCallback(const ModbusMsgInStampedConstPtr &msg_raw)
Called whenever a new modbus message arrives.
TEST(IntegrationtestExecuteBrakeTest, testBrakeTestService)
Listens to the modbus_read topic and publishes a message informing about a required brake test...
Specifies the meaning of the holding registers.


prbt_hardware_support
Author(s):
autogenerated on Mon Feb 28 2022 23:14:34