Blocking service caller

2026-07-04 08:50:35 +08:00 · 2014-04-28 00:33:08 +04:00
parent 4f544f28b7
commit ae26afdc9f
1 changed files with 64 additions and 0 deletions
@@ -172,4 +172,68 @@ static inline NodePtr makeNode(const std::vector<std::string>& iface_names)
    return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode());
 }

+/**
+ * Wrapper over uavcan::ServiceClient<> for blocking calls.
+ * Calls spin() internally.
+ */
+template <typename DataType>
+class BlockingServiceClient : public uavcan::ServiceClient<DataType>
+{
+    typedef uavcan::ServiceClient<DataType> Super;
+
+    typename DataType::Response response_;
+    bool call_was_successful_;
+
+    void callback(const uavcan::ServiceCallResult<DataType>& 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<DataType>(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_; }
+};
+
 }