diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 52955aae71..164e183c9c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -172,4 +172,68 @@ static inline NodePtr makeNode(const std::vector& iface_names) return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode()); } +/** + * Wrapper over uavcan::ServiceClient<> for blocking calls. + * Calls spin() internally. + */ +template +class BlockingServiceClient : public uavcan::ServiceClient +{ + typedef uavcan::ServiceClient Super; + + typename DataType::Response response_; + bool call_was_successful_; + + void callback(const uavcan::ServiceCallResult& res) + { + response_ = res.response; + call_was_successful_ = res.isSuccessful(); + } + + void setup() + { + Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1)); + call_was_successful_ = false; + response_ = typename DataType::Response(); + } + +public: + BlockingServiceClient(uavcan::INode& node) + : uavcan::ServiceClient(node) + , call_was_successful_(false) + { + setup(); + } + + int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request) + { + const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2); + setup(); + const int call_res = Super::call(server_node_id, request); + if (call_res >= 0) + { + while (Super::isPending()) + { + const int spin_res = Super::getNode().spin(SpinDuration); + if (spin_res < 0) + { + return spin_res; + } + } + } + return call_res; + } + + int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request, + uavcan::MonotonicDuration timeout) + { + Super::setRequestTimeout(timeout); + return blockingCall(server_node_id, request); + } + + bool wasSuccessful() const { return call_was_successful_; } + + const typename DataType::Response& getResponse() const { return response_; } +}; + }