node_info_retriever.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
 */
 
#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
 
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/multiset.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/GetNodeInfo.hpp>
 
namespace uavcan
{
class UAVCAN_EXPORT INodeInfoListener
{
public:
    virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
 
    virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
 
    virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
    {
        (void)event;
    }
 
    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
    {
        (void)msg;
    }
 
    virtual ~INodeInfoListener() { }
};
 
class UAVCAN_EXPORT NodeInfoRetriever : public NodeStatusMonitor
                                      , TimerBase
{
public:
    enum { MaxNumRequestAttempts = 254 };
    enum { UnlimitedRequestAttempts = 0 };
 
private:
    typedef MethodBinder<NodeInfoRetriever*,
                         void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
            GetNodeInfoResponseCallback;
 
    struct Entry
    {
        uint32_t uptime_sec;
        uint8_t num_attempts_made;
        bool request_needed;                    
        bool updated_since_last_attempt;        
 
        Entry()
            : uptime_sec(0)
            , num_attempts_made(0)
            , request_needed(false)
            , updated_since_last_attempt(false)
        {
#if UAVCAN_DEBUG
            StaticAssert<sizeof(Entry) <= 8>::check();
#endif
        }
    };
 
    struct NodeInfoRetrievedHandlerCaller
    {
        const NodeID node_id;
        const protocol::GetNodeInfo::Response& node_info;
 
        NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
            : node_id(arg_node_id)
            , node_info(arg_node_info)
        { }
 
        bool operator()(INodeInfoListener* key)
        {
            UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
            key->handleNodeInfoRetrieved(node_id, node_info);
            return false;
        }
    };
 
    template <typename Event>
    struct GenericHandlerCaller
    {
        void (INodeInfoListener::* const method)(Event);
        Event event;
 
        GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
            : method(arg_method)
            , event(arg_event)
        { }
 
        bool operator()(INodeInfoListener* key)
        {
            UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
            (key->*method)(event);
            return false;
        }
    };
 
    enum { DefaultNumRequestAttempts = 16 };
    enum { DefaultTimerIntervalMSec = 40 };  
 
    /*
     * State
     */
    Entry entries_[NodeID::Max];  // [1, NodeID::Max]
 
    Multiset<INodeInfoListener*> listeners_;
 
    ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
 
    MonotonicDuration request_interval_;
 
    mutable uint8_t last_picked_node_;
 
    uint8_t num_attempts_;
 
    /*
     * Methods
     */
    const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
    Entry&       getEntry(NodeID node_id)
    {
        if (node_id.get() < 1 || node_id.get() > NodeID::Max)
        {
            handleFatalError("NodeInfoRetriever NodeID");
        }
        return entries_[node_id.get() - 1];
    }
 
    void startTimerIfNotRunning()
    {
        if (!TimerBase::isRunning())
        {
            TimerBase::startPeriodic(request_interval_);
            UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
        }
    }
 
    NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
    {
        out_at_least_one_request_needed = false;
 
        for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
        {
            last_picked_node_++;
            if (last_picked_node_ > NodeID::Max)
            {
                last_picked_node_ = 1;
            }
            UAVCAN_ASSERT((last_picked_node_ >= 1) &&
                          (last_picked_node_ <= NodeID::Max));
 
            const Entry& entry = getEntry(last_picked_node_);
 
            if (entry.request_needed)
            {
                out_at_least_one_request_needed = true;
 
                if (entry.updated_since_last_attempt &&
                    !get_node_info_client_.hasPendingCallToServer(last_picked_node_))
                {
                    UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
                    return NodeID(last_picked_node_);
                }
            }
        }
 
        return NodeID();        // No node could be found
    }
 
    virtual void handleTimerEvent(const TimerEvent&)
    {
        bool at_least_one_request_needed = false;
        const NodeID next = pickNextNodeToQuery(at_least_one_request_needed);
 
        if (next.isUnicast())
        {
            UAVCAN_ASSERT(at_least_one_request_needed);
            getEntry(next).updated_since_last_attempt = false;
            const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
            if (res < 0)
            {
                get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
            }
        }
        else
        {
            if (!at_least_one_request_needed)
            {
                TimerBase::stop();
                UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped");
            }
        }
    }
 
    virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
    {
        const bool was_offline = !event.was_known ||
                                 (event.old_status.mode == protocol::NodeStatus::MODE_OFFLINE);
 
        const bool offline_now = event.status.mode == protocol::NodeStatus::MODE_OFFLINE;
 
        if (was_offline || offline_now)
        {
            Entry& entry = getEntry(event.node_id);
 
            entry.request_needed = !offline_now;
            entry.num_attempts_made = 0;
 
            UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
                         int(event.node_id.get()), int(entry.request_needed));
 
            if (entry.request_needed)
            {
                startTimerIfNotRunning();
            }
        }
 
        listeners_.forEach(
            GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
    }
 
    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
    {
        Entry& entry = getEntry(msg.getSrcNodeID());
 
        if (msg.uptime_sec < entry.uptime_sec)
        {
            entry.request_needed = true;
            entry.num_attempts_made = 0;
 
            startTimerIfNotRunning();
        }
        entry.uptime_sec = msg.uptime_sec;
        entry.updated_since_last_attempt = true;
 
        listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
            &INodeInfoListener::handleNodeStatusMessage, msg));
    }
 
    void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
    {
        Entry& entry = getEntry(result.getCallID().server_node_id);
 
        if (result.isSuccessful())
        {
            /*
             * Updating the uptime here allows to properly handle a corner case where the service response arrives
             * after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
             */
            entry.uptime_sec = result.getResponse().status.uptime_sec;
            entry.request_needed = false;
            listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id,
                                                              result.getResponse()));
        }
        else
        {
            if (num_attempts_ != UnlimitedRequestAttempts)
            {
                entry.num_attempts_made++;
                if (entry.num_attempts_made >= num_attempts_)
                {
                    entry.request_needed = false;
                    listeners_.forEach(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
                                                                    result.getCallID().server_node_id));
                }
            }
        }
    }
 
public:
    NodeInfoRetriever(INode& node)
        : NodeStatusMonitor(node)
        , TimerBase(node)
        , listeners_(node.getAllocator())
        , get_node_info_client_(node)
        , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec))
        , last_picked_node_(1)
        , num_attempts_(DefaultNumRequestAttempts)
    { }
 
    int start(const TransferPriority priority = TransferPriority::OneHigherThanLowest)
    {
        int res = NodeStatusMonitor::start();
        if (res < 0)
        {
            return res;
        }
 
        res = get_node_info_client_.init(priority);
        if (res < 0)
        {
            return res;
        }
        get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
                                                                      &NodeInfoRetriever::handleGetNodeInfoResponse));
        // Note: the timer will be started ad-hoc
        return 0;
    }
 
    void invalidateAll()
    {
        NodeStatusMonitor::forgetAllNodes();
        get_node_info_client_.cancelAllCalls();
 
        for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
        {
            entries_[i] = Entry();
        }
        // It is not necessary to reset the last picked node index
    }
 
    int addListener(INodeInfoListener* listener)
    {
        if (listener != UAVCAN_NULLPTR)
        {
            removeListener(listener);
            return (UAVCAN_NULLPTR == listeners_.emplace(listener)) ? -ErrMemory : 0;
        }
        else
        {
            return -ErrInvalidParam;
        }
    }
 
    void removeListener(INodeInfoListener* listener)
    {
        if (listener != UAVCAN_NULLPTR)
        {
            listeners_.removeAll(listener);
        }
        else
        {
            UAVCAN_ASSERT(0);
        }
    }
 
    unsigned getNumListeners() const { return listeners_.getSize(); }
 
    uint8_t getNumRequestAttempts() const { return num_attempts_; }
    void setNumRequestAttempts(const uint8_t num)
    {
        num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
    }
 
    MonotonicDuration getRequestInterval() const { return request_interval_; }
    void setRequestInterval(const MonotonicDuration interval)
    {
        if (interval.isPositive())
        {
            request_interval_ = interval;
            if (TimerBase::isRunning())
            {
                TimerBase::startPeriodic(request_interval_);
            }
        }
        else
        {
            UAVCAN_ASSERT(0);
        }
    }
 
    bool isRetrievingInProgress() const { return TimerBase::isRunning(); }
 
    uint8_t getNumPendingRequests() const
    {
        const unsigned num = get_node_info_client_.getNumPendingCalls();
        UAVCAN_ASSERT(num <= 0xFF);
        return static_cast<uint8_t>(num);
    }
};
 
}
 
#endif // Include guard