25 #include <canopen_chain_node/GetObject.h> 26 #include <canopen_chain_node/SetObject.h> 28 #include <prbt_hardware_support/BrakeTest.h> 29 #include <prbt_hardware_support/BrakeTestErrorCodes.h> 69 canopen_chain_node::GetObject srv;
70 srv.request.node = node_name;
72 srv.request.cached =
false;
74 ROS_INFO_STREAM(
"Call \"brake test duration\" service for \"" << node_name <<
"\"");
78 BrakeTestErrorCodes::GET_DURATION_FAILURE);
81 if (!srv.response.success)
84 BrakeTestErrorCodes::GET_DURATION_FAILURE);
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);
94 std::vector<ros::Duration> durations;
95 std::transform(node_names.begin(), node_names.end(), std::back_inserter(durations),
97 return *std::max_element(durations.begin(), durations.end());
102 canopen_chain_node::SetObject srv;
103 srv.request.node = node_name;
105 srv.request.value =
"1";
106 srv.request.cached =
false;
108 ROS_INFO_STREAM(
"Call \"trigger brake test\" service for \"" << node_name <<
"\"");
112 BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE);
115 if (!srv.response.success)
118 BrakeTestErrorCodes::START_BRAKE_TEST_FAILURE);
124 canopen_chain_node::GetObject srv;
125 srv.request.node = node_name;
127 srv.request.cached =
false;
129 ROS_INFO_STREAM(
"Call \"get status brake test\" service for \"" << node_name <<
"\"");
133 BrakeTestErrorCodes::GET_STATUS_FAILURE);
136 if (!srv.response.success)
139 BrakeTestErrorCodes::GET_STATUS_FAILURE);
143 status.first =
static_cast<int8_t
>(srv.response.value.data()[0]);
144 status.second = srv.response.message;
151 if (status.first != BrakeTestErrorCodes::STATUS_SUCCESS)
153 ROS_ERROR(
"Brake test for %s failed (Status: %d)", node_name.c_str(), status.first);
164 BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE);
167 std::vector<std::string> node_names;
168 for (
auto rpci = rpc.
begin(); rpci != rpc.
end(); ++rpci)
170 auto node_name = rpci->first.c_str();
179 node_names.push_back(node_name);
192 BrakeTestErrorCodes::GET_NODE_NAMES_FAILURE);
210 for (
auto node_name : node_names)
212 ROS_INFO_STREAM(
"Perform brake test for node \"" << node_name <<
"\"...");
215 max_duration.sleep();
217 std::for_each(node_names.begin(), node_names.end(),
223 response.success =
false;
225 response.error_msg = ex.what();
229 response.success =
true;
int8_t getErrorValue() const
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
const std::string & getMessage() const
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 triggerBrakeTests(BrakeTest::Request &, BrakeTest::Response &response)
bool call(MReq &req, MRes &res)
ros::ServiceClient canopen_srv_set_client_
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::ServiceClient canopen_srv_get_client_
ros::ServiceServer brake_test_srv_
Service which triggers brake tests for all joints.
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
CANOpenBrakeTestAdapter(ros::NodeHandle &nh)
const std::string & getNamespace() const
void triggerBrakeTestForNode(const std::string &node_name)
static const std::string CANOPEN_NODES_PARAMETER_NAME
std::vector< std::string > getNodeNames()
#define ROS_INFO_STREAM(args)
ros::Duration getBrakeTestDuration(const std::string &node_name)
bool getParam(const std::string &key, std::string &s) const
static const std::string CANOPEN_GETOBJECT_SERVICE_NAME
Exception thrown by the CANOpenBrakeTestAdapter.
static const std::string CANOPEN_SETOBJECT_SERVICE_NAME
std::pair< int8_t, std::string > BrakeTestStatus
#define ROS_ERROR_STREAM(args)
static const std::string GET_BRAKETEST_DURATION_OBJECT
static constexpr double WAIT_FOR_SERVICE_TIMEOUT_S
static const std::string TRIGGER_BRAKETEST_SERVICE_NAME