def _call_service_with_error_check(self, srv, srv_name, log_format,
handle_response_func, *args):
self._log_service_call(srv_name, log_format, *args)
try:
resp = srv(*args)
except rospy.ServiceException as exp:
# Retrieve the current stack
trace = "".join(traceback.format_stack()[:-1])
raise ServiceCallFailure(
RLLErrorCode.SERVICE_CALL_CLIENT_ERROR,
"Failed to call %s service: %s\n%s" % (srv_name, exp, trace))
self._last_error_code = resp.error_code
return handle_response_func(srv_name, resp)