NodeDiscoverer: fixes and test

2026-07-06 02:30:35 +08:00 · 2015-05-11 17:50:36 +03:00
parent c62b871f20
commit 558171bf71
3 changed files with 226 additions and 6 deletions
@@ -0,0 +1,185 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+#include <gtest/gtest.h>
+#include <vector>
+#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
+#include <uavcan/node/publisher.hpp>
+#include "event_tracer.hpp"
+#include "get_node_info_mock_server.hpp"
+#include "../helpers.hpp"
+
+using namespace uavcan::dynamic_node_id_server;
+
+
+class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler
+{
+public:
+    struct NodeInfo
+    {
+        UniqueID unique_id;
+        uavcan::NodeID node_id;
+        bool committed;
+
+        NodeInfo() : committed(false) { }
+    };
+
+    bool can_discover;
+    std::vector<NodeInfo> nodes;
+
+    NodeDiscoveryHandler() : can_discover(false) { }
+
+    virtual bool canDiscoverNewNodes() const
+    {
+        return can_discover;
+    }
+
+    virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
+    {
+        const NodeInfo* const ni = const_cast<NodeDiscoveryHandler*>(this)->findNode(node_id);
+        if (ni == NULL)
+        {
+            return NodeAwarenessUnknown;
+        }
+        return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
+    }
+
+    virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id)
+    {
+        NodeInfo info;
+        if (unique_id_or_null != NULL)
+        {
+            info.unique_id = *unique_id_or_null;
+        }
+        info.node_id = node_id;
+        nodes.push_back(info);
+    }
+
+    NodeInfo* findNode(const UniqueID& unique_id)
+    {
+        for (unsigned i = 0; i < nodes.size(); i++)
+        {
+            if (nodes.at(i).unique_id == unique_id)
+            {
+                return &nodes.at(i);
+            }
+        }
+        return NULL;
+    }
+
+    NodeInfo* findNode(const uavcan::NodeID& node_id)
+    {
+        for (unsigned i = 0; i < nodes.size(); i++)
+        {
+            if (nodes.at(i).node_id == node_id)
+            {
+                return &nodes.at(i);
+            }
+        }
+        return NULL;
+    }
+};
+
+
+TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
+{
+    using namespace uavcan::protocol::dynamic_node_id::server;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+
+    EventTracer tracer;
+    InterlinkedTestNodesWithSysClock nodes;
+    NodeDiscoveryHandler handler;
+
+    NodeDiscoverer disc(nodes.a, tracer, handler);
+
+    /*
+     * Initialization
+     */
+    ASSERT_LE(0, disc.init());
+
+    ASSERT_FALSE(disc.hasUnknownNodes());
+
+    /*
+     * Publishing NodeStatus, discovery is disabled
+     */
+    handler.can_discover = false;
+
+    uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
+    ASSERT_LE(0, node_status_pub.init());
+
+    uavcan::protocol::NodeStatus node_status;
+    node_status.status_code = node_status.STATUS_OK;    // Status will be ignored anyway
+    node_status.uptime_sec = 0;
+    ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));   // The timer runs as long as there are unknown nodes
+    ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled!
+    ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_TRUE(disc.hasUnknownNodes());
+
+    /*
+     * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
+     */
+    handler.can_discover = true;
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+    ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+    ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_TRUE(disc.hasUnknownNodes());
+
+    /*
+     * Publishing NodeStatus
+     */
+    node_status.uptime_sec += 5U;
+    ASSERT_LE(0, node_status_pub.broadcast(node_status));
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+    ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
+    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_TRUE(disc.hasUnknownNodes());
+
+    /*
+     * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
+     */
+    GetNodeInfoMockServer get_node_info_server(nodes.b);
+    get_node_info_server.response.hardware_version.unique_id[0] = 123;  // Arbitrary data
+    get_node_info_server.response.hardware_version.unique_id[6] = 213;
+    get_node_info_server.response.hardware_version.unique_id[14] = 52;
+    ASSERT_LE(0, get_node_info_server.start());
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
+
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
+    ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
+    ASSERT_FALSE(disc.hasUnknownNodes());
+
+    /*
+     * Checking the results
+     */
+    ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id));
+    ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get());
+}