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
+    }
+};
+
+}

libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan

@@ -1,3 +1,3 @@
-uint8[<=12] string_request
+uint8[<=64] string_request
 ---
-uint8[<=12] string_response
+uint8[<=64] string_response

libuavcan/test/service_client.cpp

@@ -0,0 +1,229 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/service_client.hpp>
+#include <uavcan/server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <root_ns_a/StringService.hpp>
+#include <queue>
+#include "clock.hpp"
+#include "internal/transport/can/can.hpp"
+
+
+template <typename DataType>
+struct ServiceCallResultHandler
+{
+    typedef typename uavcan::ServiceCallResult<DataType>::Status StatusType;
+    StatusType last_status;
+    uavcan::NodeID last_server_node_id;
+    typename DataType::Response last_response;
+
+    void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
+    {
+        // TODO: stream uavcan::ServiceCallResult<> directly
+        std::cout << "Service call result [" << DataType::getDataTypeFullName() << "], success="
+            << result.isSuccessful() << ":\n";
+        std::cout << result.response << std::endl;
+
+        last_status = result.status;
+        last_response = result.response;
+        last_server_node_id = result.server_node_id;
+    }
+
+    bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const
+    {
+        return
+            status == last_status &&
+            server_node_id == last_server_node_id &&
+            response == last_response;
+    }
+
+    typedef uavcan::MethodBinder<ServiceCallResultHandler*,
+        void (ServiceCallResultHandler::*)(const uavcan::ServiceCallResult<DataType>&)> Binder;
+
+    Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); }
+};
+
+
+static void stringServiceServerCallback(const uavcan::ReceivedDataStructure<root_ns_a::StringService::Request>& req,
+                                        root_ns_a::StringService::Response& rsp)
+{
+    rsp.string_response = "Request string: ";
+    rsp.string_response += req.string_request.c_str();
+}
+
+
+struct MakeshiftNode : uavcan::Noncopyable
+{
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<1> poolmgr;
+    SystemClockDriver clock_driver;
+    uavcan::MarshalBufferProvider<> buffer_provider;
+    uavcan::OutgoingTransferRegistry<8> otr;
+    uavcan::Scheduler scheduler;
+
+    MakeshiftNode(uavcan::ICanDriver& can_driver, uavcan::NodeID self_node_id)
+    : otr(poolmgr)
+    , scheduler(can_driver, poolmgr, clock_driver, otr, self_node_id)
+    {
+        poolmgr.addPool(&pool);
+    }
+
+    void spin(uavcan::MonotonicDuration duration)
+    {
+        scheduler.spin(clock_driver.getMonotonic() + duration);
+    }
+};
+
+
+struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
+{
+    uavcan::ISystemClock& clock;
+    PairableCanDriver* other;
+    std::queue<uavcan::CanFrame> read_queue;
+
+    PairableCanDriver(uavcan::ISystemClock& clock)
+    : clock(clock)
+    , other(NULL)
+    { }
+
+    void linkTogether(PairableCanDriver* with)
+    {
+        this->other = with;
+        with->other = this;
+    }
+
+    uavcan::ICanIface* getIface(int iface_index)
+    {
+        if (iface_index == 0)
+            return this;
+        return NULL;
+    }
+
+    int getNumIfaces() const { return 1; }
+
+    int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uavcan::MonotonicTime blocking_deadline)
+    {
+        assert(other);
+        if (inout_read_iface_mask == 1)
+            inout_read_iface_mask = read_queue.size() ? 1 : 0;
+
+        if (inout_read_iface_mask || inout_write_iface_mask)
+            return 1;
+
+        while (clock.getMonotonic() < blocking_deadline)
+            usleep(1000);
+
+        return 0;
+    }
+
+    int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime)
+    {
+        assert(other);
+        other->read_queue.push(frame);
+        return 1;
+    }
+
+    int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc)
+    {
+        assert(other);
+        assert(read_queue.size());
+        out_frame = read_queue.front();
+        read_queue.pop();
+        out_ts_monotonic = clock.getMonotonic();
+        out_ts_utc = clock.getUtc();
+        return 1;
+    }
+
+    int configureFilters(const uavcan::CanFilterConfig*, int) { return -1; }
+    int getNumFilters() const { return 0; }
+    uint64_t getNumErrors() const { return 0; }
+};
+
+
+TEST(ServiceClient, Basic)
+{
+    SystemClockDriver clock;
+
+    PairableCanDriver can_a(clock), can_b(clock);
+    can_a.linkTogether(&can_b);
+
+    MakeshiftNode node_a(can_a, 1), node_b(can_b, 2);
+
+    // Type registration
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+    // Server
+    uavcan::Server<root_ns_a::StringService> server(node_a.scheduler, node_a.poolmgr, node_a.buffer_provider);
+    ASSERT_EQ(1, server.start(stringServiceServerCallback));
+
+    {
+        // Caller
+        typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
+        typedef uavcan::ServiceClient<root_ns_a::StringService,
+            typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
+        ServiceCallResultHandler<root_ns_a::StringService> handler;
+
+        ClientType client1(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
+        ClientType client2(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
+        ClientType client3(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider);
+
+        client1.setCallback(handler.bind());
+        client2.setCallback(client1.getCallback());
+        client3.setCallback(client1.getCallback());
+        client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
+
+        ASSERT_EQ(1, node_a.scheduler.getDispatcher().getNumServiceRequestListeners());
+        ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
+
+        root_ns_a::StringService::Request request;
+        request.string_request = "Hello world";
+
+        ASSERT_LT(0, client1.call(1, request)); // OK
+        ASSERT_LT(0, client2.call(1, request)); // OK
+        ASSERT_LT(0, client3.call(99, request)); // Will timeout!
+
+        ASSERT_EQ(3, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Listening now!
+
+        ASSERT_TRUE(client1.isPending());
+        ASSERT_TRUE(client2.isPending());
+        ASSERT_TRUE(client3.isPending());
+
+        node_a.spin(uavcan::MonotonicDuration::fromMSec(10));
+        node_b.spin(uavcan::MonotonicDuration::fromMSec(10));
+
+        ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third is still listening!
+
+        ASSERT_FALSE(client1.isPending());
+        ASSERT_FALSE(client2.isPending());
+        ASSERT_TRUE(client3.isPending());
+
+        // Validating
+        root_ns_a::StringService::Response expected_response;
+        expected_response.string_response = "Request string: Hello world";
+        ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response));
+
+        node_a.spin(uavcan::MonotonicDuration::fromMSec(100));
+        node_b.spin(uavcan::MonotonicDuration::fromMSec(100));
+
+        ASSERT_FALSE(client1.isPending());
+        ASSERT_FALSE(client2.isPending());
+        ASSERT_FALSE(client3.isPending());
+
+        ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :(
+
+        // Validating
+        ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
+
+        // Stray request
+        ASSERT_LT(0, client3.call(99, request)); // Will timeout!
+        ASSERT_TRUE(client3.isPending());
+        ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners());
+    }
+
+    // All destroyed - nobody listening
+    ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners());
+}