NodeInitializer - performs network checks during intialization: NodeID collisions, incompatible data types

libuavcan/include/uavcan/protocol/node_initializer.hpp

@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <bitset>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+
+namespace uavcan
+{
+
+struct NodeInitializationResult
+{
+    NodeID conflicting_node;
+    bool isOk() const { return !conflicting_node.isValid(); }
+};
+
+/**
+ * This class does not issue GlobalDiscoveryRequest, assuming that it was done already by the caller.
+ * Instantiated object can execute() only once. Objects of this class are intended for stack allocation.
+ */
+class NodeInitializer : Noncopyable
+{
+    typedef std::bitset<NodeID::Max + 1> NodeIDMask;
+    typedef MethodBinder<NodeInitializer*,
+                         void (NodeInitializer::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
+            NodeStatusCallback;
+    typedef MethodBinder<NodeInitializer*,
+                         void (NodeInitializer::*)(ServiceCallResult<protocol::ComputeAggregateTypeSignature>&)>
+            CATSResponseCallback;
+
+    Subscriber<protocol::NodeStatus, NodeStatusCallback> ns_sub_;
+    ServiceClient<protocol::ComputeAggregateTypeSignature, CATSResponseCallback> cats_cln_;
+    NodeIDMask nid_mask_present_;
+    NodeIDMask nid_mask_checked_;
+    NodeInitializationResult result_;
+    DataTypeKind checking_dtkind_;
+    bool last_cats_request_ok_;
+
+    INode& getNode() { return ns_sub_.getNode(); }
+    const INode& getNode() const { return ns_sub_.getNode(); }
+
+    MonotonicDuration getNetworkDiscoveryDelay() const;
+
+    NodeID findNextUncheckedNode();
+
+    int waitForCATSResponse();
+
+    void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
+    void handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp);
+
+    int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind);
+    int checkOneNode(NodeID nid);
+    int checkNodes();
+
+public:
+    NodeInitializer(INode& node)
+        : ns_sub_(node)
+        , cats_cln_(node)
+        , checking_dtkind_(DataTypeKindService)
+        , last_cats_request_ok_(false)
+    { }
+
+    int execute();
+
+    const NodeInitializationResult& getResult() const { return result_; }
+
+    static int publishGlobalDiscoveryRequest(INode& node);
+};
+
+}

libuavcan/src/protocol/node_initializer.cpp

@@ -0,0 +1,196 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/node_initializer.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/protocol/GlobalDiscoveryRequest.hpp>
+
+namespace uavcan
+{
+
+MonotonicDuration NodeInitializer::getNetworkDiscoveryDelay() const
+{
+    // Base duration is constant - NodeStatus publication period
+    MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS);
+    // Additional duration depends on the node priority - gets larger with higher Node ID
+    dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10);
+    return dur;
+}
+
+NodeID NodeInitializer::findNextUncheckedNode()
+{
+    for (int i = 1; i <= NodeID::Max; i++)
+    {
+        if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i))
+        {
+            nid_mask_checked_[i] = true;
+            return NodeID(i);
+        }
+    }
+    return NodeID();
+}
+
+int NodeInitializer::waitForCATSResponse()
+{
+    while (cats_cln_.isPending())
+    {
+        const int res = getNode().spin(MonotonicDuration::fromMSec(10));
+        if (res < 0 || !result_.isOk())
+        {
+            return res;
+        }
+    }
+    return 0;
+}
+
+void NodeInitializer::handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+{
+    if (!nid_mask_present_.test(msg.getSrcNodeID().get()))
+    {
+        UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get()));
+        nid_mask_present_[msg.getSrcNodeID().get()] = true;
+    }
+
+    if (msg.getSrcNodeID() == getNode().getNodeID())
+    {
+        UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get()));
+        result_.conflicting_node = msg.getSrcNodeID();
+    }
+}
+
+void NodeInitializer::handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp)
+{
+    last_cats_request_ok_ = resp.isSuccessful();
+    if (last_cats_request_ok_)
+    {
+        const DataTypeSignature sign = GlobalDataTypeRegistry::instance().
+            computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids);
+
+        UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu",
+                     int(resp.server_node_id.get()), static_cast<unsigned long long>(sign.get()),
+                     static_cast<unsigned long long>(resp.response.aggregate_signature));
+
+        if (sign.get() != resp.response.aggregate_signature)
+        {
+            result_.conflicting_node = resp.server_node_id;
+        }
+    }
+}
+
+int NodeInitializer::checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind)
+{
+    StaticAssert<DataTypeKindMessage == int(protocol::DataTypeKind::MESSAGE)>::check();
+    StaticAssert<DataTypeKindService == int(protocol::DataTypeKind::SERVICE)>::check();
+
+    assert(nid.isUnicast());
+    assert(!cats_cln_.isPending());
+
+    checking_dtkind_ = kind;
+    protocol::ComputeAggregateTypeSignature::Request request;
+    request.kind.value = kind;
+    GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids);
+
+    int res = cats_cln_.call(nid, request);
+    if (res < 0)
+    {
+        return res;
+    }
+    res = waitForCATSResponse();
+    if (res < 0)
+    {
+        return res;
+    }
+    if (!last_cats_request_ok_)
+    {
+        return -1;
+    }
+    return 0;
+}
+
+int NodeInitializer::checkOneNode(NodeID nid)
+{
+    if (nid == getNode().getNodeID())
+    {
+        result_.conflicting_node = nid;  // NodeID collision
+        return 0;
+    }
+
+    const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage);
+    if (res < 0 || !result_.isOk())
+    {
+        return res;
+    }
+    return checkOneNodeOneDataTypeKind(nid, DataTypeKindService);
+}
+
+int NodeInitializer::checkNodes()
+{
+    nid_mask_checked_.reset();
+    while (true)
+    {
+        const NodeID nid = findNextUncheckedNode();
+        if (nid.isValid())
+        {
+            UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get()));
+            const int res = checkOneNode(nid);
+            if (res < 0 || !result_.isOk())
+            {
+                return res;
+            }
+            if (cats_cln_.getResponseFailureCount() > 0)
+            {
+                return -cats_cln_.getResponseFailureCount();
+            }
+        }
+        else { break; }
+    }
+    return 0;
+}
+
+int NodeInitializer::execute()
+{
+    int res = 0;
+
+    if (!getNode().getNodeID().isUnicast())
+    {
+        result_.conflicting_node = getNode().getNodeID();
+        goto exit;
+    }
+
+    res = ns_sub_.start(NodeStatusCallback(this, &NodeInitializer::handleNodeStatus));
+    if (res < 0)
+    {
+        goto exit;
+    }
+
+    cats_cln_.setCallback(CATSResponseCallback(this, &NodeInitializer::handleCATSResponse));
+    res = cats_cln_.init();
+    if (res < 0)
+    {
+        goto exit;
+    }
+
+    res = getNode().spin(getNetworkDiscoveryDelay());
+    if (res < 0)
+    {
+        goto exit;
+    }
+
+    res = checkNodes();
+
+exit:
+    ns_sub_.stop();
+    cats_cln_.cancel();
+    return res;
+}
+
+int NodeInitializer::publishGlobalDiscoveryRequest(INode& node)
+{
+    Publisher<protocol::GlobalDiscoveryRequest> pub(node);
+    return pub.broadcast(protocol::GlobalDiscoveryRequest());
+}
+
+}

libuavcan/test/protocol/helpers.hpp

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/timer.hpp>
 #include <uavcan/node/service_client.hpp>
 #include <uavcan/util/method_binder.hpp>
 #include "../node/test_node.hpp"
@@ -86,3 +87,19 @@ struct ServiceClientWithCollector
         return client.call(node_id, request);
     }
 };
+
+
+struct BackgroundSpinner : uavcan::TimerBase
+{
+    uavcan::INode& spinning_node;
+
+    BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node)
+        : uavcan::TimerBase(running_node)
+        , spinning_node(spinning_node)
+    { }
+
+    void handleTimerEvent(const uavcan::TimerEvent&)
+    {
+        spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1));
+    }
+};

libuavcan/test/protocol/node_initializer.cpp

@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_initializer.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include <uavcan/protocol/data_type_info_provider.hpp>
+#include <uavcan/protocol/GlobalDiscoveryRequest.hpp>
+#include <memory>
+#include "helpers.hpp"
+
+
+static void registerTypes()
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::ComputeAggregateTypeSignature> _reg3;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg4;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetDataTypeInfo> _reg5;
+}
+
+
+struct NodeInitializerRemoteContext
+{
+    uavcan::NodeStatusProvider node_status_provider;
+    uavcan::DataTypeInfoProvider data_type_info_provider;
+
+    NodeInitializerRemoteContext(uavcan::INode& node)
+        : node_status_provider(node)
+        , data_type_info_provider(node)
+    {
+        node_status_provider.setName("com.example");
+        uavcan::protocol::SoftwareVersion swver;
+        swver.build = 1;
+        node_status_provider.setSoftwareVersion(swver);
+    }
+
+    void start()
+    {
+        ASSERT_LE(0, node_status_provider.startAndPublish());
+        ASSERT_LE(0, data_type_info_provider.start());
+    }
+};
+
+
+TEST(NodeInitializer, Size)
+{
+    std::cout << "sizeof(uavcan::NodeInitializer): " << sizeof(uavcan::NodeInitializer) << std::endl;
+    ASSERT_TRUE(sizeof(uavcan::NodeInitializer) < 1024);
+}
+
+
+TEST(NodeInitializer, EmptyNetwork)
+{
+    registerTypes();
+    InterlinkedTestNodesWithSysClock nodes;
+
+    ASSERT_LE(0, uavcan::NodeInitializer::publishGlobalDiscoveryRequest(nodes.a));
+
+    uavcan::NodeInitializer ni(nodes.a);
+    ASSERT_LE(0, ni.execute());
+    ASSERT_TRUE(ni.getResult().isOk());
+}
+
+
+TEST(NodeInitializer, Success)
+{
+    registerTypes();
+    InterlinkedTestNodesWithSysClock nodes;
+    NodeInitializerRemoteContext remote(nodes.b);
+    remote.start();
+
+    BackgroundSpinner bgspinner(nodes.b, nodes.a);
+    bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10));
+
+    ASSERT_LE(0, uavcan::NodeInitializer::publishGlobalDiscoveryRequest(nodes.a));
+
+    uavcan::NodeInitializer ni(nodes.a);
+    ASSERT_LE(0, ni.execute());
+    ASSERT_TRUE(ni.getResult().isOk());
+}
+
+
+TEST(NodeInitializer, RequestTimeout)
+{
+    registerTypes();
+    InterlinkedTestNodesWithSysClock nodes;
+    NodeInitializerRemoteContext remote(nodes.b);
+    remote.start();
+
+    ASSERT_LE(0, uavcan::NodeInitializer::publishGlobalDiscoveryRequest(nodes.a));
+
+    uavcan::NodeInitializer ni(nodes.a);
+    ASSERT_GT(0, ni.execute());            // There is no background spinner, so CATS request will time out
+}
+
+
+TEST(NodeInitializer, NodeIDCollision)
+{
+    registerTypes();
+    InterlinkedTestNodesWithSysClock nodes(8, 8);   // Same NID
+    NodeInitializerRemoteContext remote(nodes.b);
+    remote.start();
+
+    BackgroundSpinner bgspinner(nodes.b, nodes.a);
+    bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10));
+
+    ASSERT_LE(0, uavcan::NodeInitializer::publishGlobalDiscoveryRequest(nodes.a));
+
+    uavcan::NodeInitializer ni(nodes.a);
+    ASSERT_LE(0, ni.execute());
+    ASSERT_FALSE(ni.getResult().isOk());
+    ASSERT_EQ(8, ni.getResult().conflicting_node.get());
+}