integrationtest_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 
21 #include <memory>
22 #include <string>
23 
24 #include <ros/ros.h>
25 #include <modbus/modbus.h>
26 
27 #include <pilz_testutils/async_test.h>
28 
30 #include <prbt_hardware_support/OperationModes.h>
34 
35 namespace prbt_hardware_support
36 {
37 using namespace modbus_api::v3;
38 
39 static const std::string TOPIC_OPERATION_MODE{ "/prbt/operation_mode" };
40 static constexpr int OPERATION_MODE_QUEUE_SIZE{ 1 };
41 
42 static const std::string OPERATION_MODE_CALLBACK_EVENT{ "operation_mode_callback_event" };
43 
48 {
49 public:
53  void initialize();
54 
55  MOCK_METHOD1(callback, void(const OperationModesConstPtr& msg));
56 
57 protected:
60 };
61 
63 {
64  subscriber_ =
65  nh_.subscribe(TOPIC_OPERATION_MODE, OPERATION_MODE_QUEUE_SIZE, &OperationModeSubscriberMock::callback, this);
66 }
67 
72 class OperationModeIntegrationTest : public testing::Test, public testing::AsyncTest
73 {
74 protected:
76  ros::NodeHandle nh_priv_{ "~" };
77 };
78 
79 MATCHER_P(IsExpectedOperationMode, exp_mode, "unexpected operation mode")
80 {
81  return arg->value == exp_mode;
82 }
83 
113 TEST_F(OperationModeIntegrationTest, testOperationModeRequestAnnouncement)
114 {
115  /**********
116  * Setup *
117  **********/
118  std::string ip;
119  int port;
120  ASSERT_TRUE(nh_priv_.getParam("modbus_server_ip", ip));
121  ASSERT_TRUE(nh_priv_.getParam("modbus_server_port", port));
122 
123  ModbusApiSpec api_spec{ nh_ };
124 
125  unsigned int modbus_register_size{ api_spec.getMaxRegisterDefinition() + 1U };
126 
127  /**********
128  * Step 1 *
129  **********/
130  prbt_hardware_support::PilzModbusServerMock modbus_server(modbus_register_size);
131 
132  std::thread modbus_server_thread(&initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
133  std::ref(modbus_server), ip.c_str(), static_cast<unsigned int>(port));
134 
135  waitForNode("/pilz_modbus_client_node");
136  waitForNode("/modbus_adapter_operation_mode_node");
137 
138  using ::testing::StrictMock;
139  StrictMock<OperationModeSubscriberMock> subscriber;
140 
141  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
142  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
143 
144  subscriber.initialize();
145 
147 
148  /**********
149  * Step 2 *
150  **********/
151  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::VERSION));
152  unsigned int version_register = api_spec.getRegisterDefinition(modbus_api_spec::VERSION);
153 
154  modbus_server.setHoldingRegister({ { version_register, MODBUS_API_VERSION_REQUIRED } });
155 
156  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::T1)))
157  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
158 
159  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::OPERATION_MODE));
160  unsigned int op_mode_register = api_spec.getRegisterDefinition(modbus_api_spec::OPERATION_MODE);
161 
162  modbus_server.setHoldingRegister({ { op_mode_register, 1 } });
163 
165 
166  /**********
167  * Step 3 *
168  **********/
169  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
170  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
171 
172  modbus_server.setHoldingRegister({ { op_mode_register, 0 } });
173 
175 
176  /**********
177  * Step 4 *
178  **********/
179  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::T2)))
180  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
181 
182  modbus_server.setHoldingRegister({ { op_mode_register, 2 } });
183 
185 
186  /**********
187  * Step 5 *
188  **********/
189  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::AUTO)))
190  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
191 
192  modbus_server.setHoldingRegister({ { op_mode_register, 3 } });
193 
195 
196  /**********
197  * Step 6 *
198  **********/
199  EXPECT_CALL(subscriber, callback(IsExpectedOperationMode(OperationModes::UNKNOWN)))
200  .WillOnce(ACTION_OPEN_BARRIER_VOID(OPERATION_MODE_CALLBACK_EVENT));
201 
202  modbus_server.setHoldingRegister({ { op_mode_register, 99 } });
203 
205 
206  /**********
207  * Step 7 *
208  **********/
209  modbus_server.terminate();
210  modbus_server_thread.join();
211 }
212 
213 } // namespace prbt_hardware_support
214 
215 int main(int argc, char* argv[])
216 {
217  ros::init(argc, argv, "integrationtest_operation_mode");
218  ros::NodeHandle nh;
219 
221  spinner.start();
222 
223  testing::InitGoogleTest(&argc, argv);
224  return RUN_ALL_TESTS();
225 }
msg
Offers a modbus server and read/write functionality via subscription/publication. ...
ROSCONSOLE_DECL void initialize()
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
static constexpr int OPERATION_MODE_QUEUE_SIZE
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static const std::string OPERATION_MODE
OperationModeIntegrationTest checks if the chain ModbusServerMock -> ModbusReadClient -> ModbusAdapte...
void spinner()
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
MATCHER_P(IsExpectedOperationMode, exp_mode, "unexpected operation mode")
void terminate()
Terminate the Server. Reading or connecting to it will fail.
static const std::string OPERATION_MODE_CALLBACK_EVENT
inline ::testing::AssertionResult waitForNode(const std::string node_name, const double loop_frequency=10.0, const ros::Duration timeout=ros::Duration(10.0))
Blocks until a node defined by node_name comes up.
Redirects callbacks of a ros::Subscriber to a mock method.
int main(int argc, char *argv[])
static const std::string TOPIC_OPERATION_MODE
void initialize()
Actual subscription takes place here.
Specifies the meaning of the holding registers.


prbt_hardware_support
Author(s):
autogenerated on Sat Jul 4 2020 03:11:50