From 96aa2956155ab6dc37dfd1f0c21442a6d8d535e1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 28 Mar 2014 00:15:19 +0400 Subject: [PATCH] NodeInitializer - performs network checks during intialization: NodeID collisions, incompatible data types --- .../uavcan/protocol/node_initializer.hpp | 76 +++++++ libuavcan/src/protocol/node_initializer.cpp | 196 ++++++++++++++++++ libuavcan/test/protocol/helpers.hpp | 17 ++ libuavcan/test/protocol/node_initializer.cpp | 116 +++++++++++ 4 files changed, 405 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/node_initializer.hpp create mode 100644 libuavcan/src/protocol/node_initializer.cpp create mode 100644 libuavcan/test/protocol/node_initializer.cpp diff --git a/libuavcan/include/uavcan/protocol/node_initializer.hpp b/libuavcan/include/uavcan/protocol/node_initializer.hpp new file mode 100644 index 0000000000..e191a16189 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_initializer.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +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 NodeIDMask; + typedef MethodBinder&)> + NodeStatusCallback; + typedef MethodBinder&)> + CATSResponseCallback; + + Subscriber ns_sub_; + ServiceClient 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& msg); + void handleCATSResponse(ServiceCallResult& 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); +}; + +} diff --git a/libuavcan/src/protocol/node_initializer.cpp b/libuavcan/src/protocol/node_initializer.cpp new file mode 100644 index 0000000000..4c9709feaa --- /dev/null +++ b/libuavcan/src/protocol/node_initializer.cpp @@ -0,0 +1,196 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + +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& 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& 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(sign.get()), + static_cast(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::check(); + StaticAssert::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 pub(node); + return pub.broadcast(protocol::GlobalDiscoveryRequest()); +} + +} diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index 586a9f960e..d508abac1d 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include #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)); + } +}; diff --git a/libuavcan/test/protocol/node_initializer.cpp b/libuavcan/test/protocol/node_initializer.cpp new file mode 100644 index 0000000000..00f984c3f9 --- /dev/null +++ b/libuavcan/test/protocol/node_initializer.cpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include "helpers.hpp" + + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _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()); +}