integrationtest_execute_brake_test.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 
20 #include <ros/ros.h>
21 
22 #include <pilz_msgs/BrakeTest.h>
23 
24 #include <pilz_testutils/joint_state_publisher_mock.h>
25 
26 #include <prbt_hardware_support/WriteModbusRegister.h>
32 
33 static const std::string EXECUTE_BRAKE_TEST_SERVICE_NAME{ "/prbt/execute_braketest" };
34 static const std::string MODBUS_WRITE_SERVICE_NAME{ "/pilz_modbus_client_node/modbus_write" };
35 static const std::string CONTROLLER_HOLD_MODE_SERVICE_NAME{ "/prbt/manipulator_joint_trajectory_controller/hold" };
36 static const std::string CONTROLLER_UNHOLD_MODE_SERVICE_NAME{ "/prbt/manipulator_joint_trajectory_controller/unhold" };
37 
38 static constexpr double WAIT_FOR_BRAKE_TEST_SERVICE_TIMEOUT_S{ 5.0 };
39 static constexpr uint16_t MODBUS_BRAKE_TEST_PREPARE_VALUE{ 1 };
40 static constexpr uint16_t MODBUS_BRAKE_TEST_EXPECTED_VALUE{ 2 };
41 
42 namespace prbt_hardware_support
43 {
44 bool brakeTestRegisterSetOnServer(PilzModbusServerMock& server, unsigned short register_perf,
45  unsigned short register_res, uint16_t expectation, float sleep_per_try_s,
46  unsigned short retries)
47 {
48  RegCont content_perf, content_res;
49  for (int i = 0; i <= retries; i++)
50  {
51  ROS_INFO("Retry %d, waited for %.1fs so far", i, sleep_per_try_s * static_cast<float>(i));
52  content_perf = server.readHoldingRegister(register_perf, 1);
53  content_res = server.readHoldingRegister(register_res, 1);
54  if (content_perf[0] == expectation && content_res[0] == expectation)
55  return true; // expected result
56  ros::Duration(sleep_per_try_s).sleep();
57  }
58  return false;
59 }
60 
82 TEST(IntegrationtestExecuteBrakeTest, testBrakeTestService)
83 {
84  ros::NodeHandle nh;
85  ros::NodeHandle nh_private("~");
86 
87  /**********
88  * Step 1 *
89  **********/
90  std::string ip;
91  int port;
92  ASSERT_TRUE(nh_private.getParam("modbus_server_ip", ip));
93  ASSERT_TRUE(nh_private.getParam("modbus_server_port", port));
94 
95  ModbusApiSpec write_api_spec{ nh, "write_api_spec/" };
96  unsigned int modbus_register_size{ write_api_spec.getMaxRegisterDefinition() + 1U };
97  prbt_hardware_support::PilzModbusServerMock modbus_server(modbus_register_size);
98  std::thread modbus_server_thread(&initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
99  std::ref(modbus_server), ip.c_str(), static_cast<unsigned int>(port));
100  ASSERT_TRUE(write_api_spec.hasRegisterDefinition(modbus_api_spec::BRAKETEST_PERFORMED));
101  unsigned short register_perf = write_api_spec.getRegisterDefinition(modbus_api_spec::BRAKETEST_PERFORMED);
102  ASSERT_TRUE(write_api_spec.hasRegisterDefinition(modbus_api_spec::BRAKETEST_RESULT));
103  unsigned short register_res = write_api_spec.getRegisterDefinition(modbus_api_spec::BRAKETEST_RESULT);
104 
105  modbus_server.setHoldingRegister({ { register_perf, MODBUS_BRAKE_TEST_PREPARE_VALUE } });
106  modbus_server.setHoldingRegister({ { register_res, MODBUS_BRAKE_TEST_PREPARE_VALUE } });
107 
108  /**********
109  * Step 2 *
110  **********/
111  CANOpenChainNodeMock canopen_mock;
112 
113  pilz_testutils::JointStatePublisherMock joint_states_pub;
114  joint_states_pub.startPublishingAsync();
115 
116  ManipulatorMock manipulator;
119 
120  /**********
121  * Step 3 *
122  **********/
123  ros::ServiceClient brake_test_srv_client = nh.serviceClient<pilz_msgs::BrakeTest>(EXECUTE_BRAKE_TEST_SERVICE_NAME);
124  EXPECT_TRUE(brake_test_srv_client.waitForExistence(ros::Duration(WAIT_FOR_BRAKE_TEST_SERVICE_TIMEOUT_S)));
125 
126  /**********
127  * Step 4 *
128  **********/
129  pilz_msgs::BrakeTest srv;
130  EXPECT_TRUE(brake_test_srv_client.call(srv));
131 
132  /**********
133  * Step 5 *
134  **********/
135  EXPECT_TRUE(brakeTestRegisterSetOnServer(modbus_server, register_perf, register_res, MODBUS_BRAKE_TEST_EXPECTED_VALUE,
136  1, 10));
137 
138  /**********
139  * Step 6 *
140  **********/
141  joint_states_pub.stopPublishing();
142  modbus_server.terminate();
143  modbus_server_thread.join();
144 }
145 
146 } // namespace prbt_hardware_support
147 
148 int main(int argc, char* argv[])
149 {
150  ros::init(argc, argv, "integrationtest_trigger_brake_test");
151  ros::NodeHandle nh;
152 
154  spinner.start();
155 
156  testing::InitGoogleTest(&argc, argv);
157  return RUN_ALL_TESTS();
158 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static const std::string MODBUS_WRITE_SERVICE_NAME
static const std::string BRAKETEST_PERFORMED
static constexpr uint16_t MODBUS_BRAKE_TEST_PREPARE_VALUE
Offers a modbus server and read/write functionality via subscription/publication. ...
Mocks the ROS Api of the manipulator relevant for stopping and holding.
std::vector< uint16_t > RegCont
Convenience data type defining the data type for a collection of registers.
RegCont readHoldingRegister(const RegCont::size_type start_index, const RegCont::size_type num_reg_to_read)
Reads the specified number of registers, beginning at the specified start point from the holding regi...
static const std::string BRAKETEST_RESULT
static constexpr uint16_t MODBUS_BRAKE_TEST_EXPECTED_VALUE
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
bool brakeTestRegisterSetOnServer(PilzModbusServerMock &server, unsigned short register_perf, unsigned short register_res, uint16_t expectation, float sleep_per_try_s, unsigned short retries)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
void spinner()
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
#define ROS_INFO(...)
bool getParam(const std::string &key, std::string &s) const
static const std::string CONTROLLER_HOLD_MODE_SERVICE_NAME
static const std::string EXECUTE_BRAKE_TEST_SERVICE_NAME
void terminate()
Terminate the Server. Reading or connecting to it will fail.
int main(int argc, char *argv[])
void advertiseHoldService(ros::NodeHandle nh, std::string hold_service_name)
void advertiseUnholdService(ros::NodeHandle nh, std::string unhold_service_name)
TEST(IntegrationtestExecuteBrakeTest, testBrakeTestService)
bool sleep() const
static const std::string CONTROLLER_UNHOLD_MODE_SERVICE_NAME
Specifies the meaning of the holding registers.
static constexpr double WAIT_FOR_BRAKE_TEST_SERVICE_TIMEOUT_S


prbt_hardware_support
Author(s):
autogenerated on Thu Sep 10 2020 03:14:36