generic_subscriber.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
00006 #define UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED
00007 
00008 #include <uavcan/error.hpp>
00009 #include <uavcan/node/abstract_node.hpp>
00010 #include <uavcan/data_type.hpp>
00011 #include <uavcan/node/global_data_type_registry.hpp>
00012 #include <uavcan/util/templates.hpp>
00013 #include <uavcan/util/lazy_constructor.hpp>
00014 #include <uavcan/debug.hpp>
00015 #include <uavcan/transport/transfer_listener.hpp>
00016 #include <uavcan/marshal/scalar_codec.hpp>
00017 #include <uavcan/marshal/types.hpp>
00018 
00019 namespace uavcan
00020 {
00021 /**
00022  * This class extends the data structure with extra information obtained from the transport layer,
00023  * such as Source Node ID, timestamps, Transfer ID, index of the interface this transfer was picked up from, etc.
00024  *
00025  * PLEASE NOTE that since this class inherits the data structure type, subscription callbacks can accept either
00026  * object of this class or the data structure type directly if the extra information is not needed.
00027  *
00028  * For example, both of these callbacks can be used with the same data structure 'Foo':
00029  *  void first(const ReceivedDataStructure<Foo>& msg);
00030  *  void second(const Foo& msg);
00031  * In the latter case, an implicit cast will happen before the callback is invoked.
00032  *
00033  * This class is not copyable because it holds a reference to a stack-allocated transfer descriptor object.
00034  * You can slice cast it to the underlying data type though, which would be copyable:
00035  *  DataType dt = rds;  // where rds is of type ReceivedDataStructure<DataType>
00036  *  // dt is now copyable
00037  */
00038 template <typename DataType_>
00039 class UAVCAN_EXPORT ReceivedDataStructure : public DataType_, Noncopyable
00040 {
00041     const IncomingTransfer* const _transfer_;   ///< Such weird name is necessary to avoid clashing with DataType fields
00042 
00043     template <typename Ret, Ret(IncomingTransfer::*Fun) () const>
00044     Ret safeget() const
00045     {
00046         if (_transfer_ == UAVCAN_NULLPTR)
00047         {
00048             return Ret();
00049         }
00050         return (_transfer_->*Fun)();
00051     }
00052 
00053 protected:
00054     ReceivedDataStructure()
00055         : _transfer_(UAVCAN_NULLPTR)
00056     { }
00057 
00058     ReceivedDataStructure(const IncomingTransfer* arg_transfer)
00059         : _transfer_(arg_transfer)
00060     {
00061         UAVCAN_ASSERT(arg_transfer != UAVCAN_NULLPTR);
00062     }
00063 
00064 public:
00065     typedef DataType_ DataType;
00066 
00067     MonotonicTime getMonotonicTimestamp() const
00068     {
00069         return safeget<MonotonicTime, &IncomingTransfer::getMonotonicTimestamp>();
00070     }
00071     UtcTime getUtcTimestamp()        const { return safeget<UtcTime, &IncomingTransfer::getUtcTimestamp>(); }
00072     TransferPriority getPriority()   const { return safeget<TransferPriority, &IncomingTransfer::getPriority>(); }
00073     TransferType getTransferType()   const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
00074     TransferID getTransferID()       const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
00075     NodeID getSrcNodeID()            const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
00076     uint8_t getIfaceIndex()          const { return safeget<uint8_t, &IncomingTransfer::getIfaceIndex>(); }
00077     bool isAnonymousTransfer()       const { return safeget<bool, &IncomingTransfer::isAnonymousTransfer>(); }
00078 };
00079 
00080 /**
00081  * This operator neatly prints the data structure prepended with extra data from the transport layer.
00082  * The extra data will be represented as YAML comment.
00083  */
00084 template <typename Stream, typename DataType>
00085 static Stream& operator<<(Stream& s, const ReceivedDataStructure<DataType>& rds)
00086 {
00087     s << "# Received struct ts_m=" << rds.getMonotonicTimestamp()
00088       << " ts_utc=" << rds.getUtcTimestamp()
00089       << " snid=" << int(rds.getSrcNodeID().get()) << "\n";
00090     s << static_cast<const DataType&>(rds);
00091     return s;
00092 }
00093 
00094 
00095 class GenericSubscriberBase : Noncopyable
00096 {
00097 protected:
00098     INode& node_;
00099     uint32_t failure_count_;
00100 
00101     explicit GenericSubscriberBase(INode& node)
00102         : node_(node)
00103         , failure_count_(0)
00104     { }
00105 
00106     ~GenericSubscriberBase() { }
00107 
00108     int genericStart(TransferListener* listener, bool (Dispatcher::*registration_method)(TransferListener*));
00109 
00110     void stop(TransferListener* listener);
00111 
00112 public:
00113     /**
00114      * Returns the number of failed attempts to decode received message. Generally, a failed attempt means either:
00115      * - Transient failure in the transport layer.
00116      * - Incompatible data types.
00117      */
00118     uint32_t getFailureCount() const { return failure_count_; }
00119 
00120     INode& getNode() const { return node_; }
00121 };
00122 
00123 /**
00124  * Please note that the reference passed to the RX callback points to a stack-allocated object, which means
00125  * that it gets invalidated shortly after the callback returns.
00126  */
00127 template <typename DataSpec, typename DataStruct, typename TransferListenerType>
00128 class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase
00129 {
00130     typedef GenericSubscriber<DataSpec, DataStruct, TransferListenerType> SelfType;
00131 
00132     // We need to break the inheritance chain here to implement lazy initialization
00133     class TransferForwarder : public TransferListenerType
00134     {
00135         SelfType& obj_;
00136 
00137         void handleIncomingTransfer(IncomingTransfer& transfer)
00138         {
00139             obj_.handleIncomingTransfer(transfer);
00140         }
00141 
00142     public:
00143         TransferForwarder(SelfType& obj,
00144                           const DataTypeDescriptor& data_type,
00145                           uint16_t max_buffer_size,
00146                           IPoolAllocator& allocator) :
00147             TransferListenerType(obj.node_.getDispatcher().getTransferPerfCounter(),
00148                                  data_type,
00149                                  max_buffer_size,
00150                                  allocator),
00151             obj_(obj)
00152         { }
00153     };
00154 
00155     LazyConstructor<TransferForwarder>  forwarder_;
00156 
00157     int checkInit();
00158 
00159     void handleIncomingTransfer(IncomingTransfer& transfer);
00160 
00161     int genericStart(bool (Dispatcher::*registration_method)(TransferListener*));
00162 
00163 protected:
00164     struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
00165     {
00166         ReceivedDataStructureSpec() { }
00167 
00168         ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) :
00169             ReceivedDataStructure<DataStruct> (arg_transfer)
00170         { }
00171     };
00172 
00173     explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node)
00174     { }
00175 
00176     virtual ~GenericSubscriber() { stop(); }
00177 
00178     virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct> &) = 0;
00179 
00180     int startAsMessageListener()
00181     {
00182         UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName());
00183         return genericStart(&Dispatcher::registerMessageListener);
00184     }
00185 
00186     int startAsServiceRequestListener()
00187     {
00188         UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s",
00189                      DataSpec::getDataTypeFullName());
00190         return genericStart(&Dispatcher::registerServiceRequestListener);
00191     }
00192 
00193     int startAsServiceResponseListener()
00194     {
00195         UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s",
00196                      DataSpec::getDataTypeFullName());
00197         return genericStart(&Dispatcher::registerServiceResponseListener);
00198     }
00199 
00200     /**
00201      * By default, anonymous transfers will be ignored.
00202      * This option allows to enable reception of anonymous transfers.
00203      */
00204     void allowAnonymousTransfers()
00205     {
00206         forwarder_->allowAnonymousTransfers();
00207     }
00208 
00209     /**
00210      * Terminate the subscription.
00211      * Dispatcher core will remove this instance from the subscribers list.
00212      */
00213     void stop()
00214     {
00215         UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName());
00216         GenericSubscriberBase::stop(forwarder_);
00217     }
00218 
00219     TransferListenerType* getTransferListener() { return forwarder_; }
00220 };
00221 
00222 // ----------------------------------------------------------------------------
00223 
00224 /*
00225  * GenericSubscriber
00226  */
00227 template <typename DataSpec, typename DataStruct, typename TransferListenerType>
00228 int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::checkInit()
00229 {
00230     if (forwarder_)
00231     {
00232         return 0;
00233     }
00234 
00235     GlobalDataTypeRegistry::instance().freeze();
00236     const DataTypeDescriptor* const descr =
00237         GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName());
00238     if (descr == UAVCAN_NULLPTR)
00239     {
00240         UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
00241         return -ErrUnknownDataType;
00242     }
00243 
00244     static const uint16_t MaxBufferSize = BitLenToByteLen<DataStruct::MaxBitLen>::Result;
00245 
00246     forwarder_.template construct<SelfType&, const DataTypeDescriptor&, uint16_t, IPoolAllocator&>
00247         (*this, *descr, MaxBufferSize, node_.getAllocator());
00248 
00249     return 0;
00250 }
00251 
00252 template <typename DataSpec, typename DataStruct, typename TransferListenerType>
00253 void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
00254 {
00255     ReceivedDataStructureSpec rx_struct(&transfer);
00256 
00257     /*
00258      * Decoding into the temporary storage
00259      */
00260     BitStream bitstream(transfer);
00261     ScalarCodec codec(bitstream);
00262 
00263     const int decode_res = DataStruct::decode(rx_struct, codec);
00264 
00265     // We don't need the data anymore, the memory can be reused from the callback:
00266     transfer.release();
00267 
00268     if (decode_res <= 0)
00269     {
00270         UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
00271                      decode_res, DataSpec::getDataTypeFullName());
00272         failure_count_++;
00273         node_.getDispatcher().getTransferPerfCounter().addError();
00274         return;
00275     }
00276 
00277     /*
00278      * Invoking the callback
00279      */
00280     handleReceivedDataStruct(rx_struct);
00281 }
00282 
00283 template <typename DataSpec, typename DataStruct, typename TransferListenerType>
00284 int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::
00285 genericStart(bool (Dispatcher::*registration_method)(TransferListener*))
00286 {
00287     const int res = checkInit();
00288     if (res < 0)
00289     {
00290         UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName());
00291         return res;
00292     }
00293     return GenericSubscriberBase::genericStart(forwarder_, registration_method);
00294 }
00295 
00296 
00297 }
00298 
00299 #endif // UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED