ServiceClient<>

libuavcan/include/uavcan/internal/node/generic_subscriber.hpp

@@ -209,6 +209,8 @@ protected:
 
     TransferListenerType* getTransferListener() { return forwarder_; }
 
+    ReceivedDataStructure<DataStruct>& getReceivedStructStorage() { return message_; }
+
 public:
     Scheduler& getScheduler() const { return scheduler_; }
 };

libuavcan/include/uavcan/internal/transport/transfer_listener.hpp

@@ -81,7 +81,7 @@ public:
 /**
  * Internal, refer to transport dispatcher.
  */
-class TransferListenerBase : public LinkedListNode<TransferListenerBase>
+class TransferListenerBase : public LinkedListNode<TransferListenerBase>, Noncopyable
 {
     const DataTypeDescriptor& data_type_;
     const TransferCRC crc_base_;                      ///< Pre-initialized with data type hash, thus constant
@@ -108,37 +108,15 @@ public:
 };
 
 /**
- * This class should be derived by transfer receivers (subscribers, servers, callers).
+ * This class should be derived by transfer receivers (subscribers, servers).
  */
 template <unsigned int MaxBufSize, unsigned int NumStaticBufs, unsigned int NumStaticReceivers>
-class TransferListener : public TransferListenerBase, Noncopyable
+class TransferListener : public TransferListenerBase
 {
     typedef TransferBufferManager<MaxBufSize, NumStaticBufs> BufferManager;
     BufferManager bufmgr_;
     Map<TransferBufferManagerKey, TransferReceiver, NumStaticReceivers> receivers_;
 
-    void handleFrame(const RxFrame& frame)
-    {
-        const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
-
-        TransferReceiver* recv = receivers_.access(key);
-        if (recv == NULL)
-        {
-            if (!frame.isFirst())
-                return;
-
-            TransferReceiver new_recv;
-            recv = receivers_.insert(key, new_recv);
-            if (recv == NULL)
-            {
-                UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
-                return;
-            }
-        }
-        TransferBufferAccessor tba(bufmgr_, key);
-        handleReception(*recv, frame, tba);
-    }
-
     class TimedOutReceiverPredicate
     {
         const MonotonicTime ts_;
@@ -174,6 +152,29 @@ class TransferListener : public TransferListenerBase, Noncopyable
         assert(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
     }
 
+protected:
+    void handleFrame(const RxFrame& frame)
+    {
+        const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
+
+        TransferReceiver* recv = receivers_.access(key);
+        if (recv == NULL)
+        {
+            if (!frame.isFirst())
+                return;
+
+            TransferReceiver new_recv;
+            recv = receivers_.insert(key, new_recv);
+            if (recv == NULL)
+            {
+                UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
+                return;
+            }
+        }
+        TransferBufferAccessor tba(bufmgr_, key);
+        handleReception(*recv, frame, tba);
+    }
+
 public:
     TransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator)
     : TransferListenerBase(data_type)
@@ -183,11 +184,78 @@ public:
         StaticAssert<(NumStaticReceivers >= NumStaticBufs)>::check();  // Otherwise it would be meaningless
     }
 
-    ~TransferListener()
+    virtual ~TransferListener()
     {
         // Map must be cleared before bufmgr is destructed
         receivers_.removeAll();
     }
 };
 
+/**
+ * This class should be derived by callers.
+ */
+template <unsigned int MaxBufSize>
+class ServiceResponseTransferListener : public TransferListener<MaxBufSize, 1, 1>
+{
+public:
+    struct ExpectedResponseParams
+    {
+        NodeID src_node_id;
+        TransferID transfer_id;
+
+        ExpectedResponseParams()
+        {
+            assert(!src_node_id.isValid());
+        }
+
+        ExpectedResponseParams(NodeID src_node_id, TransferID transfer_id)
+        : src_node_id(src_node_id)
+        , transfer_id(transfer_id)
+        {
+            assert(src_node_id.isUnicast());
+        }
+
+        bool match(const RxFrame& frame) const
+        {
+            assert(frame.getTransferType() == TransferTypeServiceResponse);
+            return (frame.getSrcNodeID() == src_node_id) && (frame.getTransferID() == transfer_id);
+        }
+    };
+
+private:
+    typedef TransferListener<MaxBufSize, 1, 1> BaseType;
+
+    ExpectedResponseParams response_params_;
+
+    void handleFrame(const RxFrame& frame)
+    {
+        if (!response_params_.match(frame))
+        {
+            UAVCAN_TRACE("ServiceResponseTransferListener", "Rejected %s [need snid=%i tid=%i]",
+                         frame.toString().c_str(),
+                         int(response_params_.src_node_id.get()), int(response_params_.transfer_id.get()));
+            return;
+        }
+        UAVCAN_TRACE("ServiceResponseTransferListener", "Accepted %s", frame.toString().c_str());
+        BaseType::handleFrame(frame);
+    }
+
+public:
+    ServiceResponseTransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator)
+    : BaseType(data_type, allocator)
+    { }
+
+    void setExpectedResponseParams(const ExpectedResponseParams& erp)
+    {
+        response_params_ = erp;
+    }
+
+    const ExpectedResponseParams& getExpectedResponseParams() const { return response_params_; }
+
+    void stopAcceptingAnything()
+    {
+        response_params_ = ExpectedResponseParams();
+    }
+};
+
 }

libuavcan/include/uavcan/service_client.hpp

@@ -0,0 +1,254 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/internal/node/generic_publisher.hpp>
+#include <uavcan/internal/node/generic_subscriber.hpp>
+
+namespace uavcan
+{
+
+template <typename ServiceDataType>
+class ServiceResponseTransferListenerInstantiationHelper
+{
+    enum { DataTypeMaxByteLen = BitLenToByteLen<ServiceDataType::Response::MaxBitLen>::Result };
+public:
+    typedef ServiceResponseTransferListener<DataTypeMaxByteLen> Type;
+};
+
+
+template <typename DataType>
+struct ServiceCallResult
+{
+    typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
+
+    enum Status { Success, ErrorTimeout };
+
+    const Status status;
+    NodeID server_node_id;
+    ResponseFieldType& response;      ///< Either response contents or unspecified response structure
+
+    ServiceCallResult(Status status, NodeID server_node_id, ResponseFieldType& response)
+    : status(status)
+    , server_node_id(server_node_id)
+    , response(response)
+    {
+        assert(server_node_id.isUnicast());
+        assert(status == Success || status == ErrorTimeout);
+    }
+
+    bool isSuccessful() const { return status == Success; }
+};
+
+
+template <typename DataType_, typename Callback = void(*)(const ServiceCallResult<DataType_>&)>
+class ServiceClient :
+    public GenericSubscriber<DataType_, typename DataType_::Response,
+                             typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >,
+    public DeadlineHandler
+{
+public:
+    typedef DataType_ DataType;
+    typedef typename DataType::Request RequestType;
+    typedef typename DataType::Response ResponseType;
+    typedef ServiceCallResult<DataType> ServiceCallResultType;
+
+private:
+    typedef ServiceClient<DataType, Callback> SelfType;
+    typedef GenericPublisher<DataType, RequestType> PublisherType;
+    typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType>::Type TransferListenerType;
+    typedef GenericSubscriber<DataType, ResponseType, TransferListenerType> SubscriberType;
+
+    PublisherType publisher_;
+    IAllocator& allocator_;
+    Callback callback_;
+    MonotonicDuration request_timeout_;
+    bool pending_;
+
+    bool isCallbackValid() const { return try_implicit_cast<bool>(callback_, true); }
+
+    void invokeCallback(ServiceCallResultType& result)
+    {
+        if (isCallbackValid())
+            callback_(result);
+        else
+            handleFatalError("Invalid caller callback");
+    }
+
+    void handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& request)
+    {
+        const TransferListenerType* const listener = SubscriberType::getTransferListener();
+        if (listener)
+        {
+            const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
+            ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, request);
+            cancel();
+            invokeCallback(result);
+        }
+        else
+        {
+            assert(0);
+            cancel();
+        }
+    }
+
+    void handleDeadline(MonotonicTime)
+    {
+        const TransferListenerType* const listener = SubscriberType::getTransferListener();
+        if (listener)
+        {
+            const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
+            ReceivedDataStructure<ResponseType>& ref = SubscriberType::getReceivedStructStorage();
+            ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref);
+
+            UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s",
+                         erp.src_node_id.get(), DataType::getDataTypeFullName());
+            cancel();
+            invokeCallback(result);
+        }
+        else
+        {
+            assert(0);
+            cancel();
+        }
+    }
+
+public:
+    ServiceClient(Scheduler& scheduler, IAllocator& allocator, IMarshalBufferProvider& buffer_provider,
+                  const Callback& callback = Callback())
+    : SubscriberType(scheduler, allocator)
+    , DeadlineHandler(scheduler)
+    , publisher_(scheduler, buffer_provider, getDefaultRequestTimeout())
+    , allocator_(allocator)
+    , callback_(callback)
+    , request_timeout_(getDefaultRequestTimeout())
+    , pending_(false)
+    {
+        setRequestTimeout(getDefaultRequestTimeout());
+#if UAVCAN_DEBUG
+        assert(getRequestTimeout() == getDefaultRequestTimeout());  // Making sure default values are OK
+#endif
+    }
+
+    virtual ~ServiceClient() { cancel(); }
+
+    int call(NodeID server_node_id, const RequestType& request) // TODO: Refactor, move into a non-template subclass!
+    {
+        /*
+         * Cancelling the pending request
+         */
+        cancel();
+        if (!isCallbackValid())
+        {
+            UAVCAN_TRACE("ServiceClient", "Invalid callback");
+            return -1;
+        }
+        pending_ = true;
+
+        /*
+         * Determining the Data Type ID
+         */
+        GlobalDataTypeRegistry::instance().freeze();
+        const DataTypeDescriptor* const descr =
+            GlobalDataTypeRegistry::instance().find(DataTypeKindService, DataType::getDataTypeFullName());
+        if (!descr)
+        {
+            UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", DataType::getDataTypeFullName());
+            cancel();
+            return -1;
+        }
+
+        /*
+         * Determining the Transfer ID
+         */
+        const OutgoingTransferRegistryKey otr_key(descr->getID(), TransferTypeServiceRequest, server_node_id);
+        const MonotonicTime otr_deadline =
+            SubscriberType::getScheduler().getMonotonicTimestamp() + TransferSender::getDefaultMaxTransferInterval();
+        TransferID* const otr_tid = SubscriberType::getScheduler().getDispatcher().getOutgoingTransferRegistry()
+            .accessOrCreate(otr_key, otr_deadline);
+        if (!otr_tid)
+        {
+            UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", descr->toString().c_str());
+            cancel();
+            return -1;
+        }
+        const TransferID transfer_id = *otr_tid;
+        otr_tid->increment();
+
+        /*
+         * Starting the subscriber
+         */
+        const int subscriber_res = SubscriberType::startAsServiceResponseListener();
+        if (subscriber_res <= 0)
+        {
+            UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
+            cancel();
+            return subscriber_res;
+        }
+
+        /*
+         * Configuring the listener so it will accept only the matching response
+         */
+        TransferListenerType* const tl = SubscriberType::getTransferListener();
+        if (!tl)
+        {
+            assert(0);  // Must have been created
+            cancel();
+            return -1;
+        }
+        const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id);
+        tl->setExpectedResponseParams(erp);
+
+        /*
+         * Registering the deadline handler
+         */
+        DeadlineHandler::startWithDelay(request_timeout_);
+
+        /*
+         * Publishing the request
+         */
+        const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id);
+        if (!publisher_res)
+        {
+            cancel();
+        }
+        return publisher_res;
+    }
+
+    void cancel()
+    {
+        pending_ = false;
+        SubscriberType::stop();
+        DeadlineHandler::stop();
+        TransferListenerType* const tl = SubscriberType::getTransferListener();
+        if (tl)
+            tl->stopAcceptingAnything();
+    }
+
+    bool isPending() const { return pending_; }
+
+    const Callback& getCallback() const { return callback_; }
+    void setCallback(const Callback& cb) { callback_ = cb; }
+
+    uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
+
+    /*
+     * Request timeouts
+     * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts
+     */
+    static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); }
+    static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
+    static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
+
+    MonotonicDuration getRequestTimeout() const { return request_timeout_; }
+    void setRequestTimeout(MonotonicDuration timeout)
+    {
+        timeout = std::max(timeout, getMinRequestTimeout());
+        timeout = std::min(timeout, getMaxRequestTimeout());
+
+        publisher_.setTxTimeout(timeout);
+        request_timeout_ = std::max(timeout, publisher_.getTxTimeout());  // No less than TX timeout
+    }
+};
+
+}