canopen_braketest_adapter.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 <algorithm>
19 #include <functional>
20 #include <sstream>
21 
22 #include <XmlRpcValue.h>
23 #include <XmlRpcException.h>
24 
25 #include <canopen_chain_node/GetObject.h>
26 #include <canopen_chain_node/SetObject.h>
27 
28 #include <prbt_hardware_support/BrakeTest.h>
29 #include <prbt_hardware_support/BrakeTestErrorCodes.h>
32 
33 namespace prbt_hardware_support
34 {
35 
36 static const std::string TRIGGER_BRAKETEST_SERVICE_NAME{"/trigger_braketest"};
37 static const std::string CANOPEN_GETOBJECT_SERVICE_NAME{"/prbt/driver/get_object"};
38 static const std::string CANOPEN_SETOBJECT_SERVICE_NAME{"/prbt/driver/set_object"};
39 static const std::string CANOPEN_NODES_PARAMETER_NAME{"/prbt/driver/nodes"};
40 static const std::string NODE_BRAKETEST_ENABLED_PARAMETER{"braketest_required"};
41 static constexpr double WAIT_FOR_SERVICE_TIMEOUT_S{5.0};
42 
43 static const std::string GET_BRAKETEST_DURATION_OBJECT{"2060sub1"};
44 static const std::string SET_START_BRAKETEST_OBJECT{"2060sub2"};
45 static const std::string GET_BRAKETEST_STATUS_OBJECT{"2060sub3"};
46 
48  : nh_(nh)
49 {
52  this);
53 
56 
58  {
59  throw CANOpenBrakeTestAdapterException("Service " + canopen_srv_get_client_.getService() + " not available.");
60  }
61  if (!canopen_srv_set_client_.waitForExistence(ros::Duration(WAIT_FOR_SERVICE_TIMEOUT_S)))
62  {
63  throw CANOpenBrakeTestAdapterException("Service " + canopen_srv_set_client_.getService() + " not available.");
64  }
65 }
66 
68 {
69  canopen_chain_node::GetObject srv;
70  srv.request.node = node_name;
71  srv.request.object = GET_BRAKETEST_DURATION_OBJECT;
72  srv.request.cached = false;
73 
74  ROS_INFO_STREAM("Call \"brake test duration\" service for \"" << node_name << "\"");
75  if (!canopen_srv_get_client_.call(srv))
76  {
77  throw CANOpenBrakeTestAdapterException("CANopen service to request brake test duration failed",
78  BrakeTestErrorCodes::GET_DURATION_FAILURE);
79  }
80 
81  if (!srv.response.success)
82  {
83  throw CANOpenBrakeTestAdapterException(srv.response.message,
84  BrakeTestErrorCodes::GET_DURATION_FAILURE);
85  }
86 
87  ROS_INFO_STREAM("Brake test duration for node \"" << node_name << "\" is: " << srv.response.value << "ms");
88  return ros::Duration(std::stoi(srv.response.value) / 1000, 0);
89 }
90 
91 ros::Duration CANOpenBrakeTestAdapter::getMaximumBrakeTestDuration(const std::vector<std::string> &node_names)
92 {
93 
94  std::vector<ros::Duration> durations;
95  std::transform(node_names.begin(), node_names.end(), std::back_inserter(durations),
96  std::bind(&CANOpenBrakeTestAdapter::getBrakeTestDuration, this, std::placeholders::_1));
97  return *std::max_element(durations.begin(), durations.end());
98 }
99 
100 void CANOpenBrakeTestAdapter::triggerBrakeTestForNode(const std::string &node_name)
101 {
102  canopen_chain_node::SetObject srv;
103  srv.request.node = node_name;
104  srv.request.object = SET_START_BRAKETEST_OBJECT;
105  srv.request.value = "1"; // Demand brake test
106  srv.request.cached = false;
107 
108  ROS_INFO_STREAM("Call \"trigger brake test\" service for \"" << node_name << "\"");
109  if (!canopen_srv_set_client_.call(srv))
110  {
111  throw CANOpenBrakeTestAdapterException("CANopen service for brake test execution failed",
112  BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE);
113  }
114 
115  if (!srv.response.success)
116  {
117  throw CANOpenBrakeTestAdapterException(srv.response.message,
118  BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE);
119  }
120 }
121 
123 {
124  canopen_chain_node::GetObject srv;
125  srv.request.node = node_name;
126  srv.request.object = GET_BRAKETEST_STATUS_OBJECT;
127  srv.request.cached = false;
128 
129  ROS_INFO_STREAM("Call \"get status brake test\" service for \"" << node_name << "\"");
130  if (!canopen_srv_get_client_.call(srv))
131  {
132  throw CANOpenBrakeTestAdapterException("CANopen service to request brake test status failed",
133  BrakeTestErrorCodes::GET_STATUS_FAILURE);
134  }
135 
136  if (!srv.response.success)
137  {
138  throw CANOpenBrakeTestAdapterException("Reading of CANopen to determine brake test status failed",
139  BrakeTestErrorCodes::GET_STATUS_FAILURE);
140  }
141 
142  BrakeTestStatus status;
143  status.first = static_cast<int8_t>(srv.response.value.data()[0]);
144  status.second = srv.response.message;
145  return status;
146 }
147 
149 {
150  BrakeTestStatus status{getBrakeTestStatusForNode(node_name)};
151  if (status.first != BrakeTestErrorCodes::STATUS_SUCCESS)
152  {
153  ROS_ERROR("Brake test for %s failed (Status: %d)", node_name.c_str(), status.first);
154  throw CANOpenBrakeTestAdapterException(status.second, status.first);
155  }
156 }
157 
158 std::vector<std::string> CANOpenBrakeTestAdapter::getNodeNames()
159 {
162  {
163  throw CANOpenBrakeTestAdapterException("Could not read node names",
164  BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE);
165  }
166 
167  std::vector<std::string> node_names;
168  for (auto& rpci : rpc)
169  {
170  auto node_name = rpci.first.c_str();
171 
172  try
173  {
174  auto braketest = rpci.second.hasMember(NODE_BRAKETEST_ENABLED_PARAMETER) &&
176 
177  if(braketest)
178  {
179  node_names.push_back(node_name);
180  ROS_INFO_STREAM("Braketest required for: " << node_name);
181  }
182  else
183  {
184  ROS_INFO_STREAM("No Braketest required for: " << node_name);
185  }
186  }
187  catch(const XmlRpc::XmlRpcException& e)
188  {
189  ROS_ERROR_STREAM("Wrong type on parameter " << NODE_BRAKETEST_ENABLED_PARAMETER << e.getMessage());
190 
191  throw CANOpenBrakeTestAdapterException("Could not read parameter " + NODE_BRAKETEST_ENABLED_PARAMETER + " of " + node_name,
192  BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE);
193  }
194 
195 
196 
197  }
198  return node_names;
199 }
200 
201 bool CANOpenBrakeTestAdapter::triggerBrakeTests(BrakeTest::Request& /*req*/, BrakeTest::Response &response)
202 {
203  try
204  {
205  auto node_names = getNodeNames();
206 
207  // should be executed prior to brake tests
208  auto max_duration = getMaximumBrakeTestDuration(node_names);
209 
210  for (const auto& node_name : node_names)
211  {
212  ROS_INFO_STREAM("Perform brake test for node \"" << node_name << "\"...");
213  triggerBrakeTestForNode(node_name);
214  }
215  max_duration.sleep();
216 
217  std::for_each(node_names.begin(), node_names.end(),
218  std::bind(&CANOpenBrakeTestAdapter::checkBrakeTestResultForNode, this, std::placeholders::_1));
219  }
220  catch (const CANOpenBrakeTestAdapterException &ex)
221  {
222  ROS_ERROR_STREAM("Brake test failed: " << ex.what());
223  response.success = false;
224  response.error_code.value = ex.getErrorValue();
225  response.error_msg = ex.what();
226  return true;
227  }
228 
229  response.success = true;
230  return true;
231 }
232 } // namespace prbt_hardware_support
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static const std::string SET_START_BRAKETEST_OBJECT
void checkBrakeTestResultForNode(const std::string &node_name)
static const std::string NODE_BRAKETEST_ENABLED_PARAMETER
BrakeTestStatus getBrakeTestStatusForNode(const std::string &node_name)
static const std::string GET_BRAKETEST_STATUS_OBJECT
bool call(MReq &req, MRes &res)
const std::string & getMessage() const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::Duration getMaximumBrakeTestDuration(const std::vector< std::string > &node_names)
ros::ServiceServer brake_test_srv_
Service which triggers brake tests for all joints.
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool getParam(const std::string &key, std::string &s) const
bool triggerBrakeTests(BrakeTest::Request &req, BrakeTest::Response &response)
void triggerBrakeTestForNode(const std::string &node_name)
static const std::string CANOPEN_NODES_PARAMETER_NAME
#define ROS_INFO_STREAM(args)
ros::Duration getBrakeTestDuration(const std::string &node_name)
const std::string & getNamespace() const
static const std::string CANOPEN_GETOBJECT_SERVICE_NAME
static const std::string CANOPEN_SETOBJECT_SERVICE_NAME
std::string getService()
#define ROS_ERROR_STREAM(args)
static const std::string GET_BRAKETEST_DURATION_OBJECT
#define ROS_ERROR(...)
static constexpr double WAIT_FOR_SERVICE_TIMEOUT_S
static const std::string TRIGGER_BRAKETEST_SERVICE_NAME


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