Subscriber with simple test

libuavcan/include/uavcan/received_data_structure.hpp

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <uavcan/internal/transport/transfer_listener.hpp>
+
+namespace uavcan
+{
+
+template <typename DataType_>
+class ReceivedDataStructure : public DataType_
+{
+    const IncomingTransfer* transfer_;
+
+    template <typename Ret, Ret (IncomingTransfer::*Fun)() const>
+    Ret safeget() const
+    {
+        if (!transfer_)
+        {
+            assert(0);
+            return Ret();
+        }
+        return (transfer_->*Fun)();
+    }
+
+protected:
+    ReceivedDataStructure() : transfer_(NULL) { }
+
+    void setTransfer(const IncomingTransfer* transfer)
+    {
+        assert(transfer);
+        transfer_ = transfer;
+    }
+
+public:
+    typedef DataType_ DataType;
+
+    uint64_t getMonotonicTimestamp() const { return safeget<uint64_t, &IncomingTransfer::getMonotonicTimestamp>(); }
+    uint64_t getUtcTimestamp()       const { return safeget<uint64_t, &IncomingTransfer::getUtcTimestamp>(); }
+    TransferType getTransferType()   const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
+    TransferID getTransferID()       const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
+    NodeID getSrcNodeID()            const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
+};
+
+}

libuavcan/include/uavcan/subscriber.hpp

@@ -0,0 +1,158 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/scheduler.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/global_data_type_registry.hpp>
+#include <uavcan/received_data_structure.hpp>
+#include <uavcan/internal/debug.hpp>
+#include <uavcan/internal/util.hpp>
+#include <uavcan/internal/lazy_constructor.hpp>
+#include <uavcan/internal/transport/transfer_listener.hpp>
+#include <uavcan/internal/marshal/scalar_codec.hpp>
+#include <uavcan/internal/marshal/types.hpp>
+
+namespace uavcan
+{
+
+template <typename DataType_,
+          typename Callback = void(*)(const ReceivedDataStructure<DataType_>&),
+          unsigned int NumStaticBufs = 1,
+          unsigned int NumStaticReceivers = NumStaticBufs + 1>
+class Subscriber : Noncopyable
+{
+    typedef Subscriber<DataType_, Callback, NumStaticBufs, NumStaticReceivers> SelfType;
+
+public:
+    typedef DataType_ DataType;
+
+private:
+    typedef TransferListener<BitLenToByteLen<DataType::MaxBitLen>::Result,
+                             NumStaticBufs ? NumStaticBufs : 1,            // TODO: add support for zero buffers
+                             NumStaticReceivers ? NumStaticReceivers : 1> TransferListenerType;
+
+    // We need to break the inheritance chain here to implement lazy initialization
+    class TransferForwarder : public TransferListenerType
+    {
+        SelfType& obj_;
+
+        void handleIncomingTransfer(IncomingTransfer& transfer)
+        {
+            obj_.handleIncomingTransfer(transfer);
+        }
+
+    public:
+        TransferForwarder(SelfType& obj, const DataTypeDescriptor& data_type, IAllocator& allocator)
+        : TransferListenerType(data_type, allocator)
+        , obj_(obj)
+        { }
+    };
+
+    struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataType>
+    {
+        using ReceivedDataStructure<DataType>::setTransfer;
+    };
+
+    Scheduler& scheduler_;
+    IAllocator& allocator_;
+    Callback callback_;
+    LazyConstructor<TransferForwarder> forwarder_;
+    ReceivedDataStructureSpec message_;
+    uint32_t failure_count_;
+
+    bool checkInit()
+    {
+        if (forwarder_)
+            return true;
+
+        GlobalDataTypeRegistry::instance().freeze();
+
+        const DataTypeDescriptor* const descr =
+            GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, DataType::getDataTypeFullName());
+        if (!descr)
+        {
+            UAVCAN_TRACE("Subscriber", "Type [%s] is not registered", DataType::getDataTypeFullName());
+            return false;
+        }
+        forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IAllocator&>(*this, *descr, allocator_);
+        return true;
+    }
+
+    void handleIncomingTransfer(IncomingTransfer& transfer)
+    {
+        assert(transfer.getTransferType() == TransferTypeMessageBroadcast ||
+               transfer.getTransferType() == TransferTypeMessageUnicast);
+
+        {
+            BitStream bitstream(transfer);
+            ScalarCodec codec(bitstream);
+
+            const int decode_res = DataType::decode(message_, codec);
+            // We don't need the data anymore, the memory can be reused from the callback:
+            transfer.release();
+            if (decode_res <= 0)
+            {
+                UAVCAN_TRACE("Subscriber", "Unable to decode the message [%i] [%s]",
+                    decode_res, DataType::getDataTypeFullName());
+                failure_count_++;
+                return;
+            }
+        }
+
+        message_.setTransfer(&transfer);
+        if (try_implicit_cast<bool>(callback_, true))
+            callback_(message_);  // Callback can accept non-const message reference and mutate it, that's OK
+        else
+            assert(0);
+    }
+
+public:
+    Subscriber(Scheduler& scheduler, IAllocator& allocator)
+    : scheduler_(scheduler)
+    , allocator_(allocator)
+    , callback_()
+    , failure_count_(0)
+    {
+        StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
+    }
+
+    int start(Callback callback)
+    {
+        stop();
+
+        if (!try_implicit_cast<bool>(callback, true))
+        {
+            UAVCAN_TRACE("Subscriber", "Invalid callback");
+            return -1;
+        }
+        callback_ = callback;
+
+        if (!checkInit())
+        {
+            UAVCAN_TRACE("Subscriber", "Initialization failure [%s]", DataType::getDataTypeFullName());
+            return -1;
+        }
+
+        if (!scheduler_.getDispatcher().registerMessageListener(forwarder_))
+        {
+            UAVCAN_TRACE("Subscriber", "Failed to register message listener [%s]", DataType::getDataTypeFullName());
+            return -1;
+        }
+        return 1;
+    }
+
+    void stop()
+    {
+        if (forwarder_)
+            scheduler_.getDispatcher().unregisterMessageListener(forwarder_);
+    }
+
+    Scheduler& getScheduler() const { return scheduler_; }
+
+    uint32_t getFailureCount() const { return failure_count_; }
+};
+
+}