void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
{
std::cout << result << std::endl;
last_status = result.getStatus();
last_response = result.getResponse();
last_server_node_id = result.getCallID().server_node_id;
responses.push(result.getResponse());
}
void handler(const uavcan::ServiceCallResult<DataType>& tmp_result)
{
std::cout << tmp_result << std::endl;
result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse()));
}