Refactoring - Scheduler, GenericPublisher, GenericSubscruber moved into /uavcan/internal/node/*

2026-10-22 10:23:34 +08:00 · 2014-03-10 13:53:10 +04:00
parent 6573d79fd1
commit 4b92497aee
10 changed files with 372 additions and 261 deletions
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/node/scheduler.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/global_data_type_registry.hpp>
+#include <uavcan/util/compile_time.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/internal/debug.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_>
+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>(); }
+};
+
+
+template <typename DataSpec, typename DataStruct, unsigned int NumStaticReceivers, unsigned int NumStaticBufs_>
+class GenericSubscriber : Noncopyable
+{
+    typedef GenericSubscriber<DataSpec, DataStruct, NumStaticReceivers, NumStaticBufs_> SelfType;
+
+    enum { DataTypeMaxByteLen = BitLenToByteLen<DataStruct::MaxBitLen>::Result };
+    enum { NeedsBuffer = int(DataTypeMaxByteLen) > int(MaxSingleFrameTransferPayloadLen) };
+    enum { BufferSize = NeedsBuffer ? DataTypeMaxByteLen : 0 };
+    enum { NumStaticBufs = NeedsBuffer ? (NumStaticBufs_ ? NumStaticBufs_ : 1) : 0 };
+
+    typedef TransferListener<BufferSize, NumStaticBufs,  // TODO: support for zero static bufs
+                             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<DataStruct>
+    {
+        using ReceivedDataStructure<DataStruct>::setTransfer;
+    };
+
+    Scheduler& scheduler_;
+    IAllocator& allocator_;
+    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(DataTypeKind(DataSpec::DataTypeKind),
+                                                    DataSpec::getDataTypeFullName());
+        if (!descr)
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
+            return false;
+        }
+        forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IAllocator&>(*this, *descr, allocator_);
+        return true;
+    }
+
+    bool decodeTransfer(IncomingTransfer& transfer)
+    {
+        BitStream bitstream(transfer);
+        ScalarCodec codec(bitstream);
+
+        message_.setTransfer(&transfer);
+
+        const int decode_res = DataSpec::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("GenericSubscriber", "Unable to decode the message [%i] [%s]",
+                         decode_res, DataSpec::getDataTypeFullName());
+            failure_count_++;
+            return false;
+        }
+        return true;
+    }
+
+    void handleIncomingTransfer(IncomingTransfer& transfer)
+    {
+        if (decodeTransfer(transfer))
+        {
+            handleReceivedDataStruct(message_);
+        }
+    }
+
+    int genericStart(bool(Dispatcher::*registration_method)(TransferListenerBase* listener))
+    {
+        stop();
+
+        if (!checkInit())
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName());
+            return -1;
+        }
+
+        if (!(scheduler_.getDispatcher().*registration_method)(forwarder_))
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener [%s]",
+                         DataSpec::getDataTypeFullName());
+            return -1;
+        }
+        return 1;
+    }
+
+protected:
+    GenericSubscriber(Scheduler& scheduler, IAllocator& allocator)
+    : scheduler_(scheduler)
+    , allocator_(allocator)
+    , failure_count_(0)
+    { }
+
+    virtual ~GenericSubscriber() { stop(); }
+
+    virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct>&) = 0;
+
+    int startAsMessageListener()
+    {
+        return genericStart(&Dispatcher::registerMessageListener);
+    }
+
+    int startAsServiceRequestListener()
+    {
+        return genericStart(&Dispatcher::registerServiceRequestListener);
+    }
+
+    int startAsServiceResponseListener()
+    {
+        return genericStart(&Dispatcher::registerServiceResponseListener);
+    }
+
+    void stop()
+    {
+        if (forwarder_)
+        {
+            scheduler_.getDispatcher().unregisterMessageListener(forwarder_);
+            scheduler_.getDispatcher().unregisterServiceRequestListener(forwarder_);
+            scheduler_.getDispatcher().unregisterServiceResponseListener(forwarder_);
+        }
+    }
+
+public:
+    Scheduler& getScheduler() const { return scheduler_; }
+
+    uint32_t getFailureCount() const { return failure_count_; }
+};
+
+}