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