unittest_modbus_adapter_operation_mode.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 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 <algorithm>
21 #include <memory>
22 
23 #include <pilz_testutils/async_test.h>
24 
28 #include <prbt_hardware_support/OperationModes.h>
31 
32 namespace prbt_hardware_support
33 {
34 using namespace modbus_api::v3;
35 
36 static constexpr int DEFAULT_QUEUE_SIZE_MODBUS{ 1 };
37 static const std::string SERVICE_NAME_OPERATION_MODE = "/prbt/get_operation_mode";
38 static const std::string TOPIC_OPERATION_MODE{ "/prbt/operation_mode" };
39 static constexpr int OPERATION_MODE_QUEUE_SIZE{ 1 };
40 
41 static const std::string OPERATION_MODE_CALLBACK_EVENT{ "operation_mode_callback_event" };
42 
44 
45 static const std::vector<uint16_t> OPERATION_MODES{ 1, 2, 3 };
46 
51 {
52 public:
56  void initialize();
57 
58  MOCK_METHOD1(callback, void(const OperationModesConstPtr& msg));
59 
60 protected:
61  ros::NodeHandle nh_;
62  ros::Subscriber subscriber_;
63 };
64 
66 {
67  subscriber_ =
68  nh_.subscribe(TOPIC_OPERATION_MODE, OPERATION_MODE_QUEUE_SIZE, &OperationModeSubscriberMock::callback, this);
69 }
70 
71 using ::testing::StrictMock;
72 
79 class ModbusAdapterOperationModeTest : public testing::Test, public testing::AsyncTest
80 {
81 public:
84 
85 protected:
86  ros::AsyncSpinner spinner_{ 2 };
88  std::shared_ptr<ModbusAdapterOperationMode> adapter_operation_mode_;
91  StrictMock<OperationModeSubscriberMock> subscriber_;
92 };
93 
95 {
96  // Initialize for ROS time if not already initialized
97  if (!ros::Time::isValid())
98  {
100  }
101 
102  adapter_operation_mode_.reset(new ModbusAdapterOperationMode(nh_, TEST_API_SPEC));
103  modbus_topic_pub_ = nh_.advertise<ModbusMsgInStamped>(TOPIC_MODBUS_READ, DEFAULT_QUEUE_SIZE_MODBUS);
104  operation_mode_client_ = nh_.serviceClient<prbt_hardware_support::GetOperationMode>(SERVICE_NAME_OPERATION_MODE);
105 
106  spinner_.start();
107 }
108 
110 {
111  // Before the destructors of the class members are called, we have
112  // to ensure that all topic and service calls done by the AsyncSpinner
113  // threads are finished. Otherwise, we sporadically will see threading
114  // exceptions like:
115  // "boost::mutex::~mutex(): Assertion `!res' failed".
116  spinner_.stop();
117 }
118 
123 TEST_F(ModbusAdapterOperationModeTest, testAdapterOperationModeDtor)
124 {
125  std::shared_ptr<AdapterOperationMode> adapter_op_mode(new AdapterOperationMode(nh_));
126 }
127 
132 TEST_F(ModbusAdapterOperationModeTest, testModbusMsgOperationModeWrapperExceptionDtor)
133 {
134  std::shared_ptr<ModbusMsgOperationModeWrapperException> ex(new ModbusMsgOperationModeWrapperException("Test "
135  "message"));
136 }
137 
142 TEST_F(ModbusAdapterOperationModeTest, testModbusMsgOperationModeWrapperDtor)
143 {
145  builder.setApiVersion(MODBUS_API_VERSION_REQUIRED).setOperationMode(OperationModes::T1);
146  std::shared_ptr<ModbusMsgOperationModeWrapper> wrapper(
148 }
149 
150 MATCHER_P(IsExpectedOperationMode, exp_mode, "unexpected operation mode")
151 {
152  return arg->value == exp_mode;
153 }
154 
160 TEST_F(ModbusAdapterOperationModeTest, testInitialOperationMode)
161 {
162  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
163  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
164 
165  subscriber_.initialize();
166 
168 
169  GetOperationMode srv;
170  ASSERT_TRUE(operation_mode_client_.call(srv));
171  EXPECT_EQ(OperationModes::UNKNOWN, srv.response.mode.value);
172 }
173 
190 TEST_F(ModbusAdapterOperationModeTest, testMissingOperationModeRegister)
191 {
192  /**********
193  * Step 1 *
194  **********/
195  ROS_DEBUG("+++ Step 1 +++");
196  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
197  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
198 
199  subscriber_.initialize();
200 
202 
203  /**********
204  * Step 2 *
205  **********/
206  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::T1)))
207  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
208 
211 
212  ROS_DEBUG("+++ Step 2 +++");
213  modbus_topic_pub_.publish(builder.setOperationMode(OperationModes::T1).build(ros::Time::now()));
214 
216 
217  /**********
218  * Step 3 *
219  **********/
220  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
221  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
222 
223  ModbusMsgInStampedPtr msg{ builder.setOperationMode(OperationModes::T1).build(ros::Time::now()) };
224 
225  ROS_DEBUG("+++ Step 3 +++");
226  // Remove operation mode from modbus message
229  << "For the test to work correctly, the operation mode register has to be stored in the last register.";
230  msg->holding_registers.data.erase(--msg->holding_registers.data.end());
231  const uint32_t new_offset = TEST_API_SPEC.getRegisterDefinition(modbus_api_spec::VERSION);
232  msg->holding_registers.layout.data_offset = new_offset;
233  ModbusMsgInBuilder::setDefaultLayout(&(msg->holding_registers.layout), new_offset,
234  static_cast<uint32_t>(msg->holding_registers.data.size()));
235 
236  modbus_topic_pub_.publish(msg);
237 
239 }
240 
256 TEST_F(ModbusAdapterOperationModeTest, testOperationModeChange)
257 {
258  /**********
259  * Step 1 *
260  **********/
261  ROS_DEBUG("+++ Step 1 +++");
262  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
263  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
264 
265  subscriber_.initialize();
266 
268 
269  /**********
270  * Step 2 *
271  **********/
274  for (const auto& mode : OPERATION_MODES)
275  {
276  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(mode)))
277  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
278 
279  modbus_topic_pub_.publish(builder.setOperationMode(mode).build(ros::Time::now()));
280 
282 
283  GetOperationMode srv;
284  ASSERT_TRUE(operation_mode_client_.call(srv));
285  EXPECT_EQ(mode, srv.response.mode.value);
286  }
287 }
288 
306 {
307  /**********
308  * Step 1 *
309  **********/
310  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
311  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
312 
313  subscriber_.initialize();
314 
316 
317  /**********
318  * Step 2 *
319  **********/
320  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::T1)))
321  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
322 
324  builder.setApiVersion(MODBUS_API_VERSION_REQUIRED).setOperationMode(OperationModes::T1);
325 
326  modbus_topic_pub_.publish(builder.build(ros::Time::now()));
327 
329 
330  /**********
331  * Step 3 *
332  **********/
333  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
334  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
335 
336  uint32_t offset{ 0 };
337  RegCont holding_register;
338  ModbusMsgInStampedPtr msg{ ModbusMsgInBuilder::createDefaultModbusMsgIn(offset, holding_register) };
339  msg->disconnect.data = true;
340  modbus_topic_pub_.publish(msg);
341 
343 }
344 
361 TEST_F(ModbusAdapterOperationModeTest, testModbusUnexpectedOperationMode)
362 {
363  /**********
364  * Step 1 *
365  **********/
366  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
367  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
368 
369  subscriber_.initialize();
370 
372 
373  /**********
374  * Step 2 *
375  **********/
376  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::T1)))
377  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
378 
380  builder.setApiVersion(MODBUS_API_VERSION_REQUIRED).setOperationMode(OperationModes::T1);
381 
382  modbus_topic_pub_.publish(builder.build(ros::Time::now()));
383 
385 
386  /**********
387  * Step 3 *
388  **********/
389  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
390  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
391 
392  builder.setApiVersion(MODBUS_API_VERSION_REQUIRED).setOperationMode(1234 /* stupid value */);
393  modbus_topic_pub_.publish(builder.build(ros::Time::now()));
394 
396 }
397 
411 TEST_F(ModbusAdapterOperationModeTest, testModbusIncorrectApiVersion)
412 {
413  /**********
414  * Step 0 *
415  **********/
416  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
417  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
418 
419  subscriber_.initialize();
420 
422 
423  /**********
424  * Step 1 *
425  **********/
426  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::T1)))
427  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
428 
430  builder.setApiVersion(MODBUS_API_VERSION_REQUIRED).setOperationMode(OperationModes::T1);
431 
432  modbus_topic_pub_.publish(builder.build(ros::Time::now()));
433 
435 
436  /**********
437  * Step 2 *
438  **********/
439  EXPECT_CALL(subscriber_, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
440  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
441 
442  builder.setApiVersion(0 /* wrong version */).setOperationMode(OperationModes::T2);
443  modbus_topic_pub_.publish(builder.build(ros::Time::now()));
444 
446 }
447 
448 } // namespace prbt_hardware_support
449 
450 int main(int argc, char* argv[])
451 {
452  ros::init(argc, argv, "unittest_modbus_adapter_operation_mode");
453  ros::NodeHandle nh;
454 
455  testing::InitGoogleTest(&argc, argv);
456  return RUN_ALL_TESTS();
457 }
msg
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
ModbusMsgInBuilder & setOperationMode(const uint16_t mode)
ROSCONSOLE_DECL void initialize()
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
static constexpr int OPERATION_MODE_QUEUE_SIZE
int main(int argc, char *argv[])
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static const std::string OPERATION_MODE
static const ModbusApiSpec TEST_API_SPEC
static const std::string TOPIC_MODBUS_READ
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
Expection thrown upon construction of ModbusMsgOperationModeWrapper of the message does not contain t...
static const std::string SERVICE_NAME_OPERATION_MODE
Listens to the modbus_read topic and offers a service informing about the active operation mode...
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 bool isValid()
MATCHER_P(IsExpectedOperationMode, exp_mode, "unexpected operation mode")
static void init()
static const std::string OPERATION_MODE_CALLBACK_EVENT
Wrapper class to add semantic to a raw ModbusMsgInStamped.
Test fixture for unit-tests of the ModbusAdapterOperationMode.
Redirects callbacks of a ros::Subscriber to a mock method.
Help on construction for ModbusMsgIn Messages.
ModbusMsgInStampedPtr build(const ros::Time &time) const
static const std::vector< uint16_t > OPERATION_MODES
static Time now()
static void setDefaultLayout(std_msgs::MultiArrayLayout *layout, const std_msgs::MultiArrayLayout::_data_offset_type &offset, const RegCont::size_type &size)
Publishes information on the active operation mode. Also offers a service for querying the operation ...
ModbusMsgInBuilder & setApiVersion(const uint16_t version)
static const std::string TOPIC_OPERATION_MODE
void initialize()
Actual subscription takes place here.
Specifies the meaning of the holding registers.
#define ROS_DEBUG(...)


prbt_hardware_support
Author(s):
autogenerated on Sun Oct 25 2020 03:13:00