integrationtest_brake_test_required.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 <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 #include <std_msgs/Bool.h>
34 
35 #include <modbus/modbus.h>
36 
37 #include <pilz_msgs/IsBrakeTestRequired.h>
38 #include <pilz_msgs/IsBrakeTestRequiredResult.h>
39 
40 #include <prbt_hardware_support/ModbusMsgInStamped.h>
44 #include <prbt_hardware_support/WriteModbusRegister.h>
45 
48 
49 #include <pilz_testutils/async_test.h>
50 
51 namespace prbt_hardware_support
52 {
53 
54 using ::testing::_;
55 using ::testing::InSequence;
56 using ::testing::Invoke;
57 using ::testing::InvokeWithoutArgs;
58 
59 static constexpr uint16_t MODBUS_API_VERSION_VALUE {2};
60 static const std::string SERVICE_BRAKETEST_REQUIRED = "/prbt/brake_test_required";
61 static const std::string MODBUS_SERVICE_NAME{"/pilz_modbus_client_node/modbus_write"};
62 
63 static constexpr unsigned int DEFAULT_RETRIES{10};
64 
71 class BrakeTestRequiredIntegrationTest : public testing::Test, public testing::AsyncTest
72 {
73 public:
74  MOCK_METHOD2(modbusWrite, bool(WriteModbusRegister::Request &, WriteModbusRegister::Response &));
75 
76 protected:
79 
81  {
83  &BrakeTestRequiredIntegrationTest::modbusWrite,
84  this)
85  };
86 
87 };
88 
89 ::testing::AssertionResult expectBrakeTestRequiredServiceCallResult
90 (ros::ServiceClient& brake_test_required_client,
91  pilz_msgs::IsBrakeTestRequiredResult::_value_type expectation,
92  uint16_t retries = DEFAULT_RETRIES)
93 {
94  pilz_msgs::IsBrakeTestRequired srv;
95  for (int i = 0; i<= retries; i++) {
96  auto res = brake_test_required_client.call(srv);
97  if(!res)
98  {
99  return ::testing::AssertionFailure() << "Could not call service";
100  }
101  if(srv.response.result.value == expectation){
102  return ::testing::AssertionSuccess() << "It took " << i+1 << " tries for the service call.";
103  }
104  sleep(1); // This then may take {retries*1}seconds.
105  }
106  return ::testing::AssertionFailure() << "Did not get expected brake test result via service";
107 }
108 
134 TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
135 {
136  /**********
137  * Setup *
138  **********/
139  std::string ip;
140  int port;
141  ASSERT_TRUE(nh_priv_.getParam("modbus_server_ip", ip));
142  ASSERT_TRUE(nh_priv_.getParam("modbus_server_port", port));
143 
144  ModbusApiSpec api_spec {nh_};
145 
146  unsigned int modbus_register_size {api_spec.getMaxRegisterDefinition() + 1U};
147 
148  /**********
149  * Step 1 *
150  **********/
151  prbt_hardware_support::PilzModbusServerMock modbus_server(modbus_register_size);
152 
153  std::thread modbus_server_thread( &initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
154  std::ref(modbus_server), ip.c_str(), static_cast<unsigned int>(port) );
155 
156  waitForNode("/pilz_modbus_client_node");
157  waitForNode("/modbus_adapter_brake_test_node");
158 
159  /**********
160  * Step 2 *
161  **********/
162  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::VERSION));
163  unsigned int version_register = api_spec.getRegisterDefinition(modbus_api_spec::VERSION);
164 
165  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::BRAKETEST_REQUEST));
166  unsigned int braketest_register = api_spec.getRegisterDefinition(modbus_api_spec::BRAKETEST_REQUEST);
167 
168  modbus_server.setHoldingRegister({{braketest_register, 1}, {version_register, MODBUS_API_VERSION_VALUE}});
169 
170  ros::ServiceClient is_brake_test_required_client =
171  nh_.serviceClient<pilz_msgs::IsBrakeTestRequired>(SERVICE_BRAKETEST_REQUIRED);
172  ASSERT_TRUE(is_brake_test_required_client.waitForExistence(ros::Duration(10)));
173 
175  is_brake_test_required_client, pilz_msgs::IsBrakeTestRequiredResult::REQUIRED));
176 
177  /**********
178  * Step 3 *
179  **********/
180  ASSERT_TRUE(api_spec.hasRegisterDefinition(modbus_api_spec::STO));
181  unsigned int sto_register = api_spec.getRegisterDefinition(modbus_api_spec::STO);
182 
183  modbus_server.setHoldingRegister({{sto_register, 1}});
184 
186  is_brake_test_required_client, pilz_msgs::IsBrakeTestRequiredResult::REQUIRED));
187 
188  /**********
189  * Step 4 *
190  **********/
191  modbus_server.setHoldingRegister({{braketest_register, 0}});
192 
194  is_brake_test_required_client, pilz_msgs::IsBrakeTestRequiredResult::NOT_REQUIRED));
195 
196  /**********
197  * Step 5 *
198  **********/
199  modbus_server.terminate();
200  modbus_server_thread.join();
201 }
202 
203 } // namespace prbt_hardware_support
204 
205 
206 int main(int argc, char *argv[])
207 {
208  ros::init(argc, argv, "integrationtest_brake_test_required");
209  ros::NodeHandle nh;
210 
211  testing::InitGoogleTest(&argc, argv);
212  return RUN_ALL_TESTS();
213 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
::testing::AssertionResult expectBrakeTestRequiredServiceCallResult(ros::ServiceClient &brake_test_required_client, pilz_msgs::IsBrakeTestRequiredResult::_value_type expectation, uint16_t retries=DEFAULT_RETRIES)
static const std::string BRAKETEST_REQUEST
Offers a modbus server and read/write functionality via subscription/publication. ...
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
int main(int argc, char *argv[])
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)
bool call(MReq &req, MRes &res)
static constexpr uint16_t MODBUS_API_VERSION_VALUE
static const std::string MODBUS_SERVICE_NAME
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static const std::string SERVICE_BRAKETEST_REQUIRED
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool getParam(const std::string &key, std::string &s) const
MOCK_METHOD2(modbusWrite, bool(WriteModbusRegister::Request &, WriteModbusRegister::Response &))
BrakeTestRequiredIntegrationTest checks if the chain ModbusServerMock -> ModbusReadClient -> ModbusAd...
void terminate()
Terminate the Server. Reading or connecting to it will fail.
static constexpr unsigned int DEFAULT_RETRIES
Specifies the meaning of the holding registers.


prbt_hardware_support
Author(s):
autogenerated on Thu Feb 13 2020 03:16:51