From 558171bf71793d02960f913a3cd1982cb588f768 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 17:50:36 +0300 Subject: [PATCH] NodeDiscoverer: fixes and test --- .../node_discoverer.hpp | 12 +- .../get_node_info_mock_server.hpp | 35 ++++ .../node_discoverer.cpp | 185 ++++++++++++++++++ 3 files changed, 226 insertions(+), 6 deletions(-) create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 7fd1474691..def1b9e9fe 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -89,7 +90,7 @@ class NodeDiscoverer : TimerBase * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. */ - enum { MaxAttemptsToGetNodeInfo = 3 }; + enum { MaxAttemptsToGetNodeInfo = 5 }; /* * States @@ -217,6 +218,7 @@ class NodeDiscoverer : TimerBase if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo) { finalizeNodeDiscovery(NULL, result.server_node_id); + // Warning: data pointer is invalidated now } } } @@ -230,15 +232,13 @@ class NodeDiscoverer : TimerBase if (!handler_.canDiscoverNewNodes()) { - trace(TraceDiscoveryTimerStop, 0); - stop(); - return; + return; // Timer must continue to run in order to not stuck when it unlocks } const NodeID node_id = pickNextNodeToQuery(); if (!node_id.isUnicast()) { - trace(TraceDiscoveryTimerStop, 1); + trace(TraceDiscoveryTimerStop, 0); stop(); return; } @@ -282,7 +282,7 @@ class NodeDiscoverer : TimerBase } data->last_seen_uptime = msg.uptime_sec; - if (!isRunning() && handler_.canDiscoverNewNodes()) + if (!isRunning()) { trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec()); startPeriodic(get_node_info_client_.getRequestTimeout()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp b/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp new file mode 100644 index 0000000000..e1da5b02ba --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +class GetNodeInfoMockServer +{ + typedef uavcan::MethodBinder&, + uavcan::protocol::GetNodeInfo::Response&) const> + GetNodeInfoCallback; + + uavcan::ServiceServer server_; + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::protocol::GetNodeInfo::Response& res) const + { + res = response; + std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl; + } + +public: + uavcan::protocol::GetNodeInfo::Response response; + + GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { } + + int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); } +}; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp new file mode 100644 index 0000000000..233c44a549 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -0,0 +1,185 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + +#include +#include +#include +#include +#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 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(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 _reg1; + uavcan::DefaultDataTypeRegistrator _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 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()); +}