brake_test_executor.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 <pilz_msgs/BrakeTest.h>
19 
21 
22 namespace prbt_hardware_support
23 {
25  ControllerHoldFunc&& controller_hold_func,
26  TriggerBrakeTestFunc&& trigger_brake_test_func, ControllerUnholdFunc&& unhold_func,
27  BrakeTestResultFunc&& brake_test_result_func)
28  : detect_robot_motion_func_(detect_robot_motion_func)
29  , hold_controller_func_(controller_hold_func)
30  , execute_brake_test_func_(trigger_brake_test_func)
31  , unhold_controller_func_(unhold_func)
32  , brake_test_result_func_(brake_test_result_func)
33 {
35  {
36  throw BrakeTestExecutorException("Function to detect robot motion missing");
37  }
38 
40  {
41  throw BrakeTestExecutorException("Function to hold controller missing");
42  }
43 
45  {
46  throw BrakeTestExecutorException("Function to trigger brake test on robot missing");
47  }
48 
50  {
51  throw BrakeTestExecutorException("Function to unhold controller missing");
52  }
53 
55  {
56  throw BrakeTestExecutorException("Function to send brake test result to FS controller missing");
57  }
58 }
59 
60 bool BrakeTestExecutor::executeBrakeTest(pilz_msgs::BrakeTest::Request& /*req*/, pilz_msgs::BrakeTest::Response& res)
61 {
63  {
64  res.success = false;
65  res.error_msg = "Robot is moving, cannot perform brake test";
66  return true;
67  }
68 
69  ROS_INFO_STREAM("Enter hold for braketest. Do not unhold the controller!");
70 
72 
73  BrakeTest::Response adapter_result = execute_brake_test_func_();
74  ROS_INFO("Brake test result: %i", adapter_result.success);
75  if (!adapter_result.success)
76  {
77  ROS_INFO("Brake test error code: %i", adapter_result.error_code.value);
78  if (!adapter_result.error_msg.empty())
79  {
80  ROS_INFO_STREAM("Brake test error msg: " << adapter_result.error_msg);
81  }
82  }
83 
85 
86  if (!brake_test_result_func_(adapter_result.success))
87  {
88  res.success = false;
89  res.error_msg = "Failed to send brake test result";
90  }
91  else
92  {
93  res.success = adapter_result.success;
94  res.error_msg = adapter_result.error_msg;
95  }
96 
97  return true;
98 }
99 
100 } // namespace prbt_hardware_support
bool executeBrakeTest(pilz_msgs::BrakeTest::Request &req, pilz_msgs::BrakeTest::Response &response)
std::function< void()> ControllerHoldFunc
std::function< BrakeTest::Response()> TriggerBrakeTestFunc
std::function< bool(const bool)> BrakeTestResultFunc
#define ROS_INFO(...)
std::function< bool()> DetectRobotMotionFunc
#define ROS_INFO_STREAM(args)
std::function< void()> ControllerUnholdFunc
BrakeTestExecutor(DetectRobotMotionFunc &&detect_robot_motion_func, ControllerHoldFunc &&controller_hold_func, TriggerBrakeTestFunc &&trigger_brake_test_func, ControllerUnholdFunc &&unhold_func, BrakeTestResultFunc &&brake_test_result_fun)


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