integrationtest_stop1.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 
21 #include <thread>
22 #include <memory>
23 #include <chrono>
24 #include <vector>
25 #include <functional>
26 #include <condition_variable>
27 #include <atomic>
28 #include <string>
29 
30 #include <ros/ros.h>
31 #include <std_srvs/SetBool.h>
32 #include <std_srvs/Trigger.h>
33 
34 #include <modbus/modbus.h>
35 
38 #include <prbt_hardware_support/ModbusMsgInStamped.h>
43 
45 
46 #include <pilz_testutils/async_test.h>
47 
48 namespace prbt_hardware_support
49 {
50 using ::testing::_;
51 using ::testing::AtLeast;
52 using ::testing::DoAll;
53 using ::testing::InSequence;
54 using ::testing::Invoke;
55 using ::testing::InvokeWithoutArgs;
56 using ::testing::Return;
57 using ::testing::SetArgReferee;
58 
59 using namespace modbus_api::v3;
60 
61 static const std::string RECOVER_SERVICE_NAME{ "recover" };
62 static const std::string HALT_SERVICE_NAME{ "halt" };
63 static const std::string HOLD_SERVICE_NAME{ "hold" };
64 static const std::string UNHOLD_SERVICE_NAME{ "unhold" };
65 
73 class Stop1IntegrationTest : public testing::Test, public testing::AsyncTest
74 {
75 public:
76  void SetUp() override;
77  void TearDown() override;
78 
79 public:
80  bool serviceCallStub(const std::string& barrier_name, const std_srvs::Trigger::Response& res_exp,
81  std_srvs::Trigger::Request& /*req*/, std_srvs::Trigger::Response& res)
82  {
83  this->triggerClearEvent(barrier_name);
84  res = res_exp;
85  return true;
86  }
87 
88 protected:
89  // Serves both the controller (/hold + /unhold) and the driver (/halt + /recover) services
91 
93  ros::NodeHandle nh_priv_{ "~" };
94 };
95 
97 {
98  manipulator.advertiseServices(nh_, HOLD_SERVICE_NAME, UNHOLD_SERVICE_NAME, HALT_SERVICE_NAME, RECOVER_SERVICE_NAME);
99 }
100 
102 {
103 }
104 
142 TEST_F(Stop1IntegrationTest, testServiceCallbacks)
143 {
144  /**********
145  * Setup *
146  **********/
147 
148  std::string ip;
149  int port;
150  ASSERT_TRUE(nh_priv_.getParam("modbus_server_ip", ip));
151  ASSERT_TRUE(nh_priv_.getParam("modbus_server_port", port));
152 
153  ModbusApiSpec api_spec{ nh_ };
154 
155  unsigned int modbus_register_size{ api_spec.getMaxRegisterDefinition() + 1U };
156 
157  /**********
158  * Step 1 *
159  **********/
160  prbt_hardware_support::PilzModbusServerMock modbus_server(static_cast<unsigned int>(modbus_register_size));
161 
162  std::thread modbus_server_thread(&initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
163  std::ref(modbus_server), ip.c_str(), static_cast<unsigned int>(port));
164 
165  waitForNode("/pilz_modbus_client_node");
166  waitForNode("/modbus_adapter_run_permitted_node");
167  waitForNode("/stop1_executor_node");
168 
169  using std::placeholders::_1;
170  using std::placeholders::_2;
171 
172  std_srvs::Trigger::Response res_exp;
173  res_exp.success = true;
174 
175  // We expect:
176  {
177  InSequence dummy;
178 
179  // Call from RUN_PERMITTED clear
180  EXPECT_CALL(manipulator, holdCb(_, _)).Times(0);
181  EXPECT_CALL(manipulator, haltCb(_, _)).Times(0);
182  EXPECT_CALL(manipulator, recoverCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
183  EXPECT_CALL(manipulator, unholdCb(_, _))
184  .Times(1)
185  .WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "unhold_callback", res_exp, _1, _2)));
186  // Expected came true -> go on
187  }
188 
189  // This should trigger the expected reaction
190  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::VERSION));
191  unsigned int version_register = api_spec.getRegisterDefinition(modbus_api_spec::VERSION);
192 
193  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::RUN_PERMITTED));
194  unsigned int run_permitted_register = api_spec.getRegisterDefinition(modbus_api_spec::RUN_PERMITTED);
195 
196  modbus_server.setHoldingRegister(
197  { { version_register, MODBUS_API_VERSION_REQUIRED }, { run_permitted_register, MODBUS_RUN_PERMITTED_TRUE } });
198 
199  /**********
200  * Step 2 *
201  **********/
202  BARRIER("unhold_callback");
203 
204  {
205  InSequence dummy;
206 
207  // Call from RUN_PERMITTED active
208  EXPECT_CALL(manipulator, unholdCb(_, _)).Times(0);
209  EXPECT_CALL(manipulator, recoverCb(_, _)).Times(0);
210  EXPECT_CALL(manipulator, holdCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
211  EXPECT_CALL(manipulator, haltCb(_, _))
212  .Times(1)
213  .WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "halt_callback", res_exp, _1, _2)));
214  }
215 
216  modbus_server.setHoldingRegister({ { run_permitted_register, MODBUS_RUN_PERMITTED_FALSE } });
217 
218  /**********
219  * Step 3 *
220  **********/
221  BARRIER("halt_callback");
222 
223  {
224  InSequence dummy;
225 
226  // Call from RUN_PERMITTED clear
227  EXPECT_CALL(manipulator, holdCb(_, _)).Times(0);
228  EXPECT_CALL(manipulator, haltCb(_, _)).Times(0);
229  EXPECT_CALL(manipulator, recoverCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
230  EXPECT_CALL(manipulator, unholdCb(_, _))
231  .Times(1)
232  .WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "unhold_callback", res_exp, _1, _2)));
233  }
234 
235  modbus_server.setHoldingRegister({ { run_permitted_register, MODBUS_RUN_PERMITTED_TRUE } });
236 
237  /**********
238  * Step 4 *
239  **********/
240  BARRIER("unhold_callback");
241  {
242  InSequence dummy;
243 
244  // Call from Disconnect
245  EXPECT_CALL(manipulator, holdCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
246  EXPECT_CALL(manipulator, unholdCb(_, _)).Times(0);
247  EXPECT_CALL(manipulator, recoverCb(_, _)).Times(0);
248  EXPECT_CALL(manipulator, haltCb(_, _))
249  .Times(1)
250  .WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "halt_callback", res_exp, _1, _2)));
251  }
252 
253  modbus_server.terminate();
254  modbus_server_thread.join();
255 
256  /**********
257  * Step 5 *
258  **********/
259  BARRIER("halt_callback"); // Needed for proper async finish
260 }
261 
262 } // namespace prbt_hardware_support
263 
264 int main(int argc, char* argv[])
265 {
266  ros::init(argc, argv, "integrationtest_stop1");
267  ros::NodeHandle nh;
268 
270  spinner.start();
271 
272  testing::InitGoogleTest(&argc, argv);
273  return RUN_ALL_TESTS();
274 }
int main(int argc, char *argv[])
Offers a modbus server and read/write functionality via subscription/publication. ...
Mocks the ROS Api of the manipulator relevant for stopping and holding.
static constexpr uint16_t MODBUS_RUN_PERMITTED_FALSE
static const std::string HOLD_SERVICE_NAME
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
Stop1IntegrationTest checks if the chain ModbusServerMock -> ModbusReadClient -> RunPermittedModbusAd...
static const std::string UNHOLD_SERVICE_NAME
static const std::string RECOVER_SERVICE_NAME
static const std::string RUN_PERMITTED
static const std::string HALT_SERVICE_NAME
void terminate()
Terminate the Server. Reading or connecting to it will fail.
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.
bool serviceCallStub(const std::string &barrier_name, const std_srvs::Trigger::Response &res_exp, std_srvs::Trigger::Request &, std_srvs::Trigger::Response &res)
static constexpr uint16_t MODBUS_RUN_PERMITTED_TRUE
Specifies the meaning of the holding registers.


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