NodeStatusMonitor

libuavcan/include/uavcan/protocol/node_status_monitor.hpp

@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+
+namespace uavcan
+{
+
+class NodeStatusMonitor : protected Timer
+{
+public:
+    typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::status_code>::Type NodeStatusCode;
+
+    struct NodeStatus
+    {
+        NodeStatusCode status_code;  ///< Current status code; OFFLINE if not known.
+        bool known;                  ///< True if the node was online at least once.
+
+        NodeStatus()
+            : status_code(protocol::NodeStatus::STATUS_OFFLINE)
+            , known(false)
+        { }
+    };
+
+    struct NodeStatusChangeEvent
+    {
+        NodeID node_id;
+        NodeStatus status;
+        NodeStatus old_status;
+    };
+
+private:
+    enum { TimerPeriodMs100 = 5 };
+
+    typedef MethodBinder<NodeStatusMonitor*,
+                         void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
+            NodeStatusCallback;
+
+    Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;
+
+    struct Entry
+    {
+        NodeStatusCode status_code;
+        int8_t time_since_last_update_ms100;
+        Entry()
+            : status_code(protocol::NodeStatus::STATUS_OFFLINE)
+            , time_since_last_update_ms100(-1)
+        { }
+    };
+
+    mutable Entry entries_[NodeID::Max];  // [1, NodeID::Max]
+
+    Entry& getEntry(NodeID node_id) const;
+
+    void changeNodeStatus(const NodeID node_id, const Entry new_entry_value);
+
+    void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
+
+    void handleTimerEvent(const TimerEvent&);
+
+protected:
+    /**
+     * Called when a node becomes online, changes status or goes offline.
+     */
+    virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
+    {
+        (void)event;
+    }
+
+    /**
+     * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange().
+     */
+    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+    {
+        (void)msg;
+    }
+
+public:
+    explicit NodeStatusMonitor(INode& node)
+        : Timer(node)
+        , sub_(node)
+    { }
+
+    virtual ~NodeStatusMonitor() { }
+
+    int start();
+
+    void forgetNode(NodeID node_id);
+
+    NodeStatus getNodeStatus(NodeID node_id) const;
+
+    NodeID findNodeWithWorstStatus() const;
+};
+
+}

libuavcan/src/protocol/node_status_monitor.cpp

@@ -0,0 +1,143 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/debug.hpp>
+#include <uavcan/fatal_error.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+
+namespace uavcan
+{
+
+NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const
+{
+    if (node_id.get() < 1 || node_id.get() > NodeID::Max)
+    {
+        handleFatalError("NodeStatusMonitor NodeID out of range");
+    }
+    return entries_[node_id.get() - 1];
+}
+
+void NodeStatusMonitor::changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
+{
+    Entry& entry = getEntry(node_id);
+    if (entry.status_code != new_entry_value.status_code)
+    {
+        NodeStatusChangeEvent event;
+        event.node_id = node_id;
+
+        event.old_status.known = entry.time_since_last_update_ms100 >= 0;
+        event.old_status.status_code = entry.status_code;
+
+        event.status.known = true;
+        event.status.status_code = new_entry_value.status_code;
+
+        UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()),
+                     (event.old_status.known ? "known" : "new"),
+                     int(event.old_status.status_code), int(event.status.status_code));
+        handleNodeStatusChange(event);
+    }
+    entry = new_entry_value;
+}
+
+void NodeStatusMonitor::handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+{
+    Entry new_entry_value;
+    new_entry_value.time_since_last_update_ms100 = 0;
+    new_entry_value.status_code = msg.status_code;
+
+    changeNodeStatus(msg.getSrcNodeID(), new_entry_value);
+
+    handleNodeStatusMessage(msg);
+}
+
+void NodeStatusMonitor::handleTimerEvent(const TimerEvent&)
+{
+    enum { OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100 };
+
+    for (int i = 1; i <= NodeID::Max; i++)
+    {
+        const NodeID nid(i);
+        assert(nid.isUnicast());
+        Entry& entry = getEntry(nid);
+        if (entry.time_since_last_update_ms100 >= 0 &&
+            entry.status_code != protocol::NodeStatus::STATUS_OFFLINE)
+        {
+            entry.time_since_last_update_ms100 += TimerPeriodMs100;
+            if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
+            {
+                Entry new_entry_value;
+                new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
+                new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE;
+                changeNodeStatus(nid, new_entry_value);
+            }
+        }
+    }
+}
+
+int NodeStatusMonitor::start()
+{
+    const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
+    if (res >= 0)
+    {
+        Timer::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
+    }
+    return res;
+}
+
+void NodeStatusMonitor::forgetNode(NodeID node_id)
+{
+    if (node_id.isValid())
+    {
+        Entry& entry = getEntry(node_id);
+        entry = Entry();
+    }
+    else
+    {
+        assert(0);
+    }
+}
+
+NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const
+{
+    if (!node_id.isValid())
+    {
+        assert(0);
+        return NodeStatus();
+    }
+    NodeStatus status;
+    const Entry& entry = getEntry(node_id);
+    if (entry.time_since_last_update_ms100 >= 0)
+    {
+        status.known = true;
+        status.status_code = entry.status_code;
+    }
+    return status;
+}
+
+NodeID NodeStatusMonitor::findNodeWithWorstStatus() const
+{
+    NodeID nid_with_worst_status;
+    NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
+
+    for (int i = 1; i <= NodeID::Max; i++)
+    {
+        const NodeID nid(i);
+        assert(nid.isUnicast());
+        const Entry& entry = getEntry(nid);
+        if (entry.time_since_last_update_ms100 >= 0)
+        {
+            if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid())
+            {
+                nid_with_worst_status = nid;
+                worst_status_code = entry.status_code;
+            }
+        }
+    }
+    return nid_with_worst_status;
+}
+
+}

libuavcan/test/protocol/node_status_monitor.cpp

@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, uavcan::uint8_t status_code,
+                              uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
+{
+    uavcan::StaticTransferBuffer<100> buffer;
+    uavcan::BitStream bitstream(buffer);
+    uavcan::ScalarCodec codec(bitstream);
+
+    uavcan::protocol::NodeStatus msg;
+    msg.status_code = status_code;
+    msg.uptime_sec = uptime_sec;
+
+    // Manual message publication
+    ASSERT_LT(0, uavcan::protocol::NodeStatus::encode(msg, codec));
+    ASSERT_GE(7, buffer.getMaxWritePos());
+
+    // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
+    // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
+    uavcan::Frame frame(uavcan::protocol::NodeStatus::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+                        node_id, uavcan::NodeID::Broadcast, 0, tid, true);
+
+    ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos()));
+
+    uavcan::CanFrame can_frame;
+    ASSERT_TRUE(frame.compile(can_frame));
+
+    for (int i = 0; i < can.getNumIfaces(); i++)
+    {
+        can.ifaces.at(i).pushRx(can_frame);
+    }
+}
+
+
+static void shortSpin(TestNode& node)
+{
+    ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+}
+
+
+TEST(NodeStatusMonitor, Basic)
+{
+    using uavcan::protocol::NodeStatus;
+    using uavcan::NodeID;
+
+    SystemClockMock clock_mock(100);
+    clock_mock.monotonic_auto_advance = 1000;
+
+    CanDriverMock can(2, clock_mock);
+
+    TestNode node(can, clock_mock, 64);
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+
+    uavcan::NodeStatusMonitor nsm(node);
+    ASSERT_LE(0, nsm.start());
+
+    ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+
+    /*
+     * Empty NSM, no nodes were registered yet
+     */
+    ASSERT_FALSE(nsm.findNodeWithWorstStatus().isValid());
+
+    uavcan::NodeStatusMonitor::NodeStatus st = nsm.getNodeStatus(uavcan::NodeID(123));
+    ASSERT_FALSE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    /*
+     * Some new status messages
+     */
+    publishNodeStatus(can, 10, NodeStatus::STATUS_OK, 12, 0);
+    shortSpin(node);
+    ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstStatus());
+
+    publishNodeStatus(can, 9,  NodeStatus::STATUS_INITIALIZING, 0, 0);
+    shortSpin(node);
+    ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus());
+
+    publishNodeStatus(can, 11, NodeStatus::STATUS_CRITICAL, 999, 0);
+    shortSpin(node);
+    ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus());
+
+    st = nsm.getNodeStatus(uavcan::NodeID(10));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(9));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(11));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
+
+    /*
+     * Timeout
+     */
+    std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl;
+
+    clock_mock.advance(1000000);
+    shortSpin(node);
+    st = nsm.getNodeStatus(uavcan::NodeID(10));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
+
+    clock_mock.advance(1000000);
+    shortSpin(node);
+    st = nsm.getNodeStatus(uavcan::NodeID(9));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
+
+    clock_mock.advance(1000000);
+    shortSpin(node);
+    st = nsm.getNodeStatus(uavcan::NodeID(11));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
+
+    /*
+     * Will timeout now
+     */
+    clock_mock.advance(2000000);
+    shortSpin(node);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(10));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(9));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(11));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    /*
+     * Recovering one node, adding two extra
+     */
+    publishNodeStatus(can, 11, NodeStatus::STATUS_WARNING, 999, 0);
+    shortSpin(node);
+
+    publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 0);
+    shortSpin(node);
+
+    publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 0);
+    shortSpin(node);
+
+    /*
+     * Making sure OFFLINE is still worst status
+     */
+    ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus());
+
+    /*
+     * Final validation
+     */
+    st = nsm.getNodeStatus(uavcan::NodeID(10));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(9));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(11));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(127));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
+
+    st = nsm.getNodeStatus(uavcan::NodeID(1));
+    ASSERT_TRUE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
+
+    /*
+     * Forgetting
+     */
+    nsm.forgetNode(127);
+    st = nsm.getNodeStatus(uavcan::NodeID(127));
+    ASSERT_FALSE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+
+    nsm.forgetNode(9);
+    st = nsm.getNodeStatus(uavcan::NodeID(9));
+    ASSERT_FALSE(st.known);
+    ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
+}