From 6b733dde49d3bb9a79dc44afe56e183c0df9e2e9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 28 Mar 2014 13:21:56 +0400 Subject: [PATCH] Main Node class --- libuavcan/include/uavcan/node/node.hpp | 221 +++++++++++++++++++++++++ libuavcan/test/node/node.cpp | 90 ++++++++++ 2 files changed, 311 insertions(+) create mode 100644 libuavcan/include/uavcan/node/node.hpp create mode 100644 libuavcan/test/node/node.cpp diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp new file mode 100644 index 0000000000..6e08d470a6 --- /dev/null +++ b/libuavcan/include/uavcan/node/node.hpp @@ -0,0 +1,221 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +// High-level functionality available by default +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ + +template +class Node : public INode +{ + enum + { + MemPoolSize = (MemPoolSize_ < std::size_t(MemPoolBlockSize)) ? std::size_t(MemPoolBlockSize) : MemPoolSize_ + }; + + PoolAllocator pool_allocator_; + MarshalBufferProvider marsh_buf_; + OutgoingTransferRegistry outgoing_trans_reg_; + Scheduler scheduler_; + + DataTypeInfoProvider proto_dtp_; + Logger proto_logger_; + NodeStatusProvider proto_nsp_; + RestartRequestServer proto_rrs_; + TransportStatsProvider proto_tsp_; + + bool started_; + + int initNetwork(NodeInitializationResult& node_init_result) + { + int res = NodeInitializer::publishGlobalDiscoveryRequest(*this); + if (res < 0) + { + return res; + } + NodeInitializer initializer(*this); + StaticAssert<(sizeof(initializer) < 1024)>::check(); + res = initializer.execute(); + node_init_result = initializer.getResult(); + return res; + } + +protected: + virtual void registerInternalFailure(const char* msg) + { + UAVCAN_TRACE("Node", "Internal failure: %s", msg); + (void)logError("UAVCAN", msg); + } + + virtual IAllocator& getAllocator() { return pool_allocator_; } + + virtual IMarshalBufferProvider& getMarshalBufferProvider() { return marsh_buf_; } + +public: + Node(ICanDriver& can_driver, ISystemClock& system_clock) + : outgoing_trans_reg_(pool_allocator_) + , scheduler_(can_driver, pool_allocator_, system_clock, outgoing_trans_reg_) + , proto_dtp_(*this) + , proto_logger_(*this) + , proto_nsp_(*this) + , proto_rrs_(*this) + , proto_tsp_(*this) + , started_(false) + { } + + virtual Scheduler& getScheduler() { return scheduler_; } + virtual const Scheduler& getScheduler() const { return scheduler_; } + + int spin(MonotonicTime deadline) + { + if (started_) + { + return INode::spin(deadline); + } + return -1; + } + + int spin(MonotonicDuration duration) + { + if (started_) + { + return INode::spin(duration); + } + return -1; + } + + bool isStarted() const { return started_; } + + int start(NodeInitializationResult& node_init_result) + { + if (started_) + { + return 0; + } + GlobalDataTypeRegistry::instance().freeze(); + + int res = 0; + res = proto_dtp_.start(); + if (res < 0) + { + goto fail; + } + res = proto_logger_.init(); + if (res < 0) + { + goto fail; + } + res = proto_nsp_.startAndPublish(); + if (res < 0) + { + goto fail; + } + res = proto_rrs_.start(); + if (res < 0) + { + goto fail; + } + res = proto_tsp_.start(); + if (res < 0) + { + goto fail; + } + res = initNetwork(node_init_result); + started_ = (res >= 0) && node_init_result.isOk(); + return res; + fail: + assert(res < 0); + return res; + } + + /* + * Initialization methods + */ + void setName(const char* name) { proto_nsp_.setName(name); } + + void setStatusOk() { proto_nsp_.setStatusOk(); } + void setStatusInitializing() { proto_nsp_.setStatusInitializing(); } + void setStatusWarning() { proto_nsp_.setStatusWarning(); } + void setStatusCritical() { proto_nsp_.setStatusCritical(); } + void setStatusOffline() + { + proto_nsp_.setStatusOffline(); + (void)proto_nsp_.forcePublish(); + } + + void setSoftwareVersion(const protocol::SoftwareVersion& version) { proto_nsp_.setSoftwareVersion(version); } + void setHardwareVersion(const protocol::HardwareVersion& version) { proto_nsp_.setHardwareVersion(version); } + + NodeStatusProvider& getNodeStatusProvider() { return proto_nsp_; } + + /* + * Restart handler + */ + void setRestartRequestHandler(IRestartRequestHandler* handler) { proto_rrs_.setHandler(handler); } + + RestartRequestServer& getRestartRequestServer() { return proto_rrs_; } + + /* + * Logging + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + + template + inline int logDebug(const char* source, const char* format, Args... args) + { + return proto_logger_.logDebug(source, format, args...); + } + + template + inline int logInfo(const char* source, const char* format, Args... args) + { + return proto_logger_.logInfo(source, format, args...); + } + + template + inline int logWarning(const char* source, const char* format, Args... args) + { + return proto_logger_.logWarning(source, format, args...); + } + + template + inline int logError(const char* source, const char* format, Args... args) + { + return proto_logger_.logError(source, format, args...); + } + +#else + + int logDebug(const char* source, const char* text) { return proto_logger_.logDebug(source, text); } + int logInfo(const char* source, const char* text) { return proto_logger_.logInfo(source, text); } + int logWarning(const char* source, const char* text) { return proto_logger_.logWarning(source, text); } + int logError(const char* source, const char* text) { return proto_logger_.logError(source, text); } + +#endif + + Logger& getLogger() { return proto_logger_; } +}; + +} diff --git a/libuavcan/test/node/node.cpp b/libuavcan/test/node/node.cpp new file mode 100644 index 0000000000..6d6d8115b8 --- /dev/null +++ b/libuavcan/test/node/node.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "test_node.hpp" +#include "../protocol/helpers.hpp" + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + uavcan::DefaultDataTypeRegistrator _reg8; +} + + +TEST(Node, Basic) +{ + registerTypes(); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 0; + swver.minor = 1; + swver.build = 0xDEADBEEF; + + /* + * uavcan::Node + */ + uavcan::Node<0> node1(nodes.can_a, nodes.clock_a); + std::cout << "sizeof(uavcan::Node<0>): " << sizeof(uavcan::Node<0>) << std::endl; + node1.setName("com.example"); + node1.setNodeID(1); + node1.setSoftwareVersion(swver); + + /* + * Companion test node + */ + uavcan::Node<0> node2(nodes.can_b, nodes.clock_b); + node2.setName("foobar"); + node2.setNodeID(2); + node2.setSoftwareVersion(swver); + + BackgroundSpinner bgspinner(node2, node1); + bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + + uavcan::NodeStatusMonitor node_status_monitor(node2); + ASSERT_LE(0, node_status_monitor.start()); + + /* + * Init the second node - network is empty + */ + uavcan::NodeInitializationResult result; + ASSERT_LE(0, node2.start(result)); + + ASSERT_FALSE(node_status_monitor.findNodeWithWorstStatus().isValid()); + + /* + * Init the first node + */ + ASSERT_FALSE(node1.isStarted()); + ASSERT_EQ(-1, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); // Not initialized - will fail + ASSERT_LE(0, node1.start(result)); + ASSERT_TRUE(node1.isStarted()); + + ASSERT_EQ(1, node_status_monitor.findNodeWithWorstStatus().get()); + + /* + * Some logging + */ + SubscriberWithCollector log_sub(node2); + ASSERT_LE(0, log_sub.start()); + + node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + ASSERT_LE(1, node1.logInfo("test", "6 * 9 = 42")); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20))); + + ASSERT_TRUE(log_sub.collector.msg.get()); + std::cout << *log_sub.collector.msg << std::endl; +}