diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 4aa98056aa..3e1315a9af 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -460,6 +460,11 @@ public: */ virtual void onEntryCommitted(const Entry& entry) = 0; + /** + * Assume false by default. + */ + virtual void onLeaderChange(bool local_node_is_leader) = 0; + virtual ~ILeaderLogCommitHandler() { } }; @@ -655,7 +660,7 @@ public: inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const { UAVCAN_ASSERT(try_implicit_cast(predicate, true)); - for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index--; index >= 0) + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) { const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); UAVCAN_ASSERT(entry != NULL); @@ -737,8 +742,8 @@ public: /** * This class implements the top-level allocation logic and server API. */ -class DynamicNodeIDAllocationServer : public dynamic_node_id_server_impl::IAllocationRequestHandler - , public dynamic_node_id_server_impl::ILeaderLogCommitHandler +class DynamicNodeIDAllocationServer : private dynamic_node_id_server_impl::IAllocationRequestHandler + , private dynamic_node_id_server_impl::ILeaderLogCommitHandler { typedef MethodBinder 0)) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); @@ -1062,6 +1067,12 @@ void RaftCore::switchState(const ServerState new_state) int(server_state_), int(new_state)); trace(TraceRaftStateSwitch, new_state); + if ((ServerStateLeader == server_state_) || + (ServerStateLeader == new_state)) + { + log_commit_handler_.onLeaderChange(ServerStateLeader == new_state); + } + server_state_ = new_state; cluster_.resetAllServerIndices(); @@ -1721,18 +1732,137 @@ int AllocationRequestManager::broadcastAllocationResponse(const IAllocationReque return allocation_pub_.broadcast(msg); } +/* + * There's no reason to remove these types from the class scope, except that otherwise the half-broken Eclipse CDT + * indexer goes bananas. + */ +struct UniqueIDLogPredicate +{ + const IAllocationRequestHandler::UniqueID unique_id; + + UniqueIDLogPredicate(const IAllocationRequestHandler::UniqueID& uid) + : unique_id(uid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.unique_id == unique_id; + } +}; + +struct NodeIDLogPredicate +{ + const NodeID node_id; + + NodeIDLogPredicate(const NodeID& nid) + : node_id(nid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.node_id == node_id.get(); + } +}; + } // dynamic_node_id_server_impl /* * DynamicNodeIDAllocationServer */ -void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) +bool DynamicNodeIDAllocationServer::isNodeIDTaken(const NodeID node_id) const { - // TODO implement proper allocation logic - (void)raft_core_.appendLog(unique_id, preferred_node_id); + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Testing if node ID %d is taken", int(node_id.get())); + return raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::NodeIDLogPredicate(node_id)); +} + +NodeID DynamicNodeIDAllocationServer::findFreeNodeID(const NodeID preferred_node_id) const +{ + uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + + // Up + while (candidate <= NodeID::Max) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + candidate--; // This has been tested already + + // Down + while (candidate > 0) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); +} + +void DynamicNodeIDAllocationServer::allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) +{ + const NodeID allocated_node_id = findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "New node ID allocated: %d", int(allocated_node_id.get())); + const int res = raft_core_.appendLog(unique_id, allocated_node_id); + if (res < 0) + { + getNode().registerInternalFailure("Raft log append"); + } +} + +void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) +{ + // TODO: allocation requests must not be served if the list of unidentified nodes is not empty + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("DynamicNodeIDAllocationServer", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("DynamicNodeIDAllocationServer", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + allocateNewNode(unique_id, preferred_node_id); + } } void DynamicNodeIDAllocationServer::onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) +{ + tryPublishAllocationResult(entry); +} + +void DynamicNodeIDAllocationServer::onLeaderChange(bool local_node_is_leader) +{ + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "I am leader: %d", int(local_node_is_leader)); + allocation_request_manager_.setActive(local_node_is_leader); +} + +void DynamicNodeIDAllocationServer::tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) { const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); if (res < 0) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 840f4b5adc..47da8124f3 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -85,6 +85,11 @@ class CommitHandler : public uavcan::dynamic_node_id_server_impl::ILeaderLogComm std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; } + virtual void onLeaderChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + public: CommitHandler(const std::string& id) : id_(id) { } }; @@ -1040,6 +1045,53 @@ TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) } +TEST(DynamicNodeIDAllocationServer, Main) +{ + using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + + EventTracer tracer; + StorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + /* + * Server + */ + uavcan::DynamicNodeIDAllocationServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(1)); + + /* + * Client + */ + uavcan::DynamicNodeIDAllocationClient client(nodes.b); + uavcan::protocol::HardwareVersion hwver; + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(hwver, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} + + TEST(DynamicNodeIDAllocationServer, ObjectSizes) { std::cout << "Log: "