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 
51 using ::testing::_;
52 using ::testing::AtLeast;
53 using ::testing::InSequence;
54 using ::testing::Invoke;
55 using ::testing::InvokeWithoutArgs;
56 using ::testing::Return;
57 using ::testing::DoAll;
58 using ::testing::SetArgReferee;
59 
60 static constexpr uint16_t MODBUS_API_VERSION_VALUE {2};
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 
76 public:
77  void SetUp() override;
78  void TearDown() override;
79 
80 public:
81  bool serviceCallStub (const std::string& barrier_name,
82  const std_srvs::Trigger::Response& res_exp,
83  std_srvs::Trigger::Request& /*req*/,
84  std_srvs::Trigger::Response& res)
85  {
86  this->triggerClearEvent(barrier_name);
87  res = res_exp;
88  return true;
89  }
90 
91 protected:
92  // Serves both the controller (/hold + /unhold) and the driver (/halt + /recover) services
94 
97 };
98 
100 {
106 }
107 
109 {
110 }
111 
149 TEST_F(Stop1IntegrationTest, testServiceCallbacks)
150 {
151  /**********
152  * Setup *
153  **********/
154 
155  std::string ip;
156  int port;
157  ASSERT_TRUE(nh_priv_.getParam("modbus_server_ip", ip));
158  ASSERT_TRUE(nh_priv_.getParam("modbus_server_port", port));
159 
160  ModbusApiSpec api_spec {nh_};
161 
162  unsigned int modbus_register_size {api_spec.getMaxRegisterDefinition() + 1U};
163 
164  /**********
165  * Step 1 *
166  **********/
167  prbt_hardware_support::PilzModbusServerMock modbus_server(static_cast<unsigned int>(modbus_register_size));
168 
169  std::thread modbus_server_thread( &initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
170  std::ref(modbus_server), ip.c_str(), static_cast<unsigned int>(port) );
171 
172  waitForNode("/pilz_modbus_client_node");
173  waitForNode("/modbus_adapter_sto_node");
174  waitForNode("/stop1_executor_node");
175 
176  using std::placeholders::_1;
177  using std::placeholders::_2;
178 
179  std_srvs::Trigger::Response res_exp;
180  res_exp.success = true;
181 
182  // We expect:
183  {
184  InSequence dummy;
185 
186  // Call from STO clear
187  EXPECT_CALL(manipulator, holdCb(_,_)).Times(0);
188  EXPECT_CALL(manipulator, haltCb(_,_)).Times(0);
189  EXPECT_CALL(manipulator, recoverCb(_,_)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
190  EXPECT_CALL(manipulator, unholdCb(_,_)).Times(1).WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "unhold_callback", res_exp, _1, _2)));
191  // Expected came true -> go on
192  }
193 
194  // This should trigger the expected reaction
195  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::VERSION));
196  unsigned int version_register = api_spec.getRegisterDefinition(modbus_api_spec::VERSION);
197 
198  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::STO));
199  unsigned int sto_register = api_spec.getRegisterDefinition(modbus_api_spec::STO);
200 
201  modbus_server.setHoldingRegister({{version_register, MODBUS_API_VERSION_VALUE},
202  {sto_register, modbus_api::v2::MODBUS_STO_CLEAR_VALUE}});
203 
204  /**********
205  * Step 2 *
206  **********/
207  BARRIER("unhold_callback");
208 
209  {
210  InSequence dummy;
211 
212  // Call from STO active
213  EXPECT_CALL(manipulator, unholdCb(_,_)).Times(0);
214  EXPECT_CALL(manipulator, recoverCb(_,_)).Times(0);
215  EXPECT_CALL(manipulator, holdCb(_,_)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
216  EXPECT_CALL(manipulator, haltCb(_,_)).Times(1).WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "halt_callback", res_exp, _1, _2)));
217  }
218 
219  modbus_server.setHoldingRegister({{sto_register, modbus_api::v2::MODBUS_STO_ACTIVE_VALUE}});
220 
221  /**********
222  * Step 3 *
223  **********/
224  BARRIER("halt_callback");
225 
226  {
227  InSequence dummy;
228 
229  // Call from STO clear
230  EXPECT_CALL(manipulator, holdCb(_,_)).Times(0);
231  EXPECT_CALL(manipulator, haltCb(_,_)).Times(0);
232  EXPECT_CALL(manipulator, recoverCb(_,_)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
233  EXPECT_CALL(manipulator, unholdCb(_,_)).Times(1).WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "unhold_callback", res_exp, _1, _2)));
234  }
235 
236  modbus_server.setHoldingRegister({{sto_register, modbus_api::v2::MODBUS_STO_CLEAR_VALUE}});
237 
238  /**********
239  * Step 4 *
240  **********/
241  BARRIER("unhold_callback");
242  {
243  InSequence dummy;
244 
245  // Call from Disconnect
246  EXPECT_CALL(manipulator, holdCb(_,_)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(true)));
247  EXPECT_CALL(manipulator, unholdCb(_,_)).Times(0);
248  EXPECT_CALL(manipulator, recoverCb(_,_)).Times(0);
249  EXPECT_CALL(manipulator, haltCb(_,_)).Times(1).WillOnce(Invoke(std::bind(&Stop1IntegrationTest::serviceCallStub, this, "halt_callback", res_exp, _1, _2)));
250 
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 
265 int main(int argc, char *argv[])
266 {
267  ros::init(argc, argv, "integrationtest_stop1");
268  ros::NodeHandle nh;
269 
271  spinner.start();
272 
273  testing::InitGoogleTest(&argc, argv);
274  return RUN_ALL_TESTS();
275 }
int main(int argc, char *argv[])
Offers a modbus server and read/write functionality via subscription/publication. ...
static constexpr uint16_t MODBUS_STO_CLEAR_VALUE
Mocks the ROS Api of the manipulator relevant for stopping and holding.
static const std::string HOLD_SERVICE_NAME
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
void waitForNode(std::string node_name, double loop_frequency=10.0)
Blocks until a node defined by node_name comes up.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static constexpr uint16_t MODBUS_API_VERSION_VALUE
void spinner()
Stop1IntegrationTest checks if the chain ModbusServerMock -> ModbusReadClient -> StoModbusAdapter -> ...
static const std::string UNHOLD_SERVICE_NAME
void advertiseServices(ros::NodeHandle nh, std::string hold_service_name, std::string unhold_service_name, std::string halt_service_name, std::string recover_service_name)
static const std::string RECOVER_SERVICE_NAME
static const std::string HALT_SERVICE_NAME
bool getParam(const std::string &key, std::string &s) const
void terminate()
Terminate the Server. Reading or connecting to it will fail.
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_STO_ACTIVE_VALUE
Specifies the meaning of the holding registers.


prbt_hardware_support
Author(s):
autogenerated on Fri Feb 28 2020 03:16:57