From 730a571c8d0544ee97571332374e77419707ff03 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Mar 2014 14:49:23 +0400 Subject: [PATCH] The work on the highest-level concepts have just started; here goes NodeStatusProvider with tests --- .../uavcan/protocol/node_status_provider.hpp | 86 ++++++++++++++ .../src/protocol/node_status_provider.cpp | 112 ++++++++++++++++++ libuavcan/test/protocol/helpers.hpp | 89 ++++++++++++++ .../test/protocol/node_status_provider.cpp | 108 +++++++++++++++++ 4 files changed, 395 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/node_status_provider.hpp create mode 100644 libuavcan/src/protocol/node_status_provider.cpp create mode 100644 libuavcan/test/protocol/helpers.hpp create mode 100644 libuavcan/test/protocol/node_status_provider.cpp diff --git a/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/libuavcan/include/uavcan/protocol/node_status_provider.hpp new file mode 100644 index 0000000000..deeafcd3db --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class NodeStatusProvider : private Timer +{ + typedef MethodBinder + GlobalDiscoveryRequestCallback; + typedef MethodBinder GetNodeInfoCallback; + + const MonotonicTime creation_timestamp_; + + Publisher node_status_pub_; + Subscriber gdr_sub_; + ServiceServer gni_srv_; + + protocol::GetNodeInfo::Response node_info_; + + INode& getNode() { return node_status_pub_.getNode(); } + + bool isNodeInfoInitialized() const; + + int publish(); + void publishWithErrorHandling(); + + void handleTimerEvent(const TimerEvent&); + void handleGlobalDiscoveryRequest(const protocol::GlobalDiscoveryRequest&); + void handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp); + +public: + NodeStatusProvider(INode& node) + : Timer(node) + , creation_timestamp_(node.getMonotonicTime()) + , node_status_pub_(node) + , gdr_sub_(node) + , gni_srv_(node) + { + assert(!creation_timestamp_.isZero()); + + node_info_.uavcan_version.major = UAVCAN_VERSION_MAJOR; + node_info_.uavcan_version.minor = UAVCAN_VERSION_MINOR; + node_info_.uavcan_version.build = UAVCAN_VERSION_BUILD; + + node_info_.status.status_code = protocol::NodeStatus::STATUS_INITIALIZING; + } + + int startAndPublish(); + + int forcePublish() { return publish(); } + + uint8_t getStatusCode() const { return node_info_.status.status_code; } + void setStatusCode(uint8_t code); + void setStatusOk() { setStatusCode(protocol::NodeStatus::STATUS_OK); } + void setStatusInitializing() { setStatusCode(protocol::NodeStatus::STATUS_INITIALIZING); } + void setStatusWarning() { setStatusCode(protocol::NodeStatus::STATUS_WARNING); } + void setStatusCritical() { setStatusCode(protocol::NodeStatus::STATUS_CRITICAL); } + void setStatusOffline() { setStatusCode(protocol::NodeStatus::STATUS_OFFLINE); } + + const typename protocol::GetNodeInfo::Response::FieldTypes::name& getName() const { return node_info_.name; } + void setName(const char* name); + + const protocol::SoftwareVersion& getSoftwareVersion() const { return node_info_.software_version; } + const protocol::HardwareVersion& getHardwareVersion() const { return node_info_.hardware_version; } + void setSoftwareVersion(const protocol::SoftwareVersion& version); + void setHardwareVersion(const protocol::HardwareVersion& version); +}; + +} diff --git a/libuavcan/src/protocol/node_status_provider.cpp b/libuavcan/src/protocol/node_status_provider.cpp new file mode 100644 index 0000000000..a494b381b9 --- /dev/null +++ b/libuavcan/src/protocol/node_status_provider.cpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +bool NodeStatusProvider::isNodeInfoInitialized() const +{ + // Hardware version is not required + return (node_info_.software_version != protocol::SoftwareVersion()) && + (node_info_.uavcan_version != protocol::SoftwareVersion()) && + (!node_info_.name.empty()); +} + +int NodeStatusProvider::publish() +{ + const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_; + assert(uptime.isPositive()); + node_info_.status.uptime_sec = uptime.toMSec() / 1000; + + assert(node_info_.status.status_code <= protocol::NodeStatus::FieldTypes::status_code::max()); + + return node_status_pub_.broadcast(node_info_.status); +} + +void NodeStatusProvider::publishWithErrorHandling() +{ + const int res = publish(); + if (res < 0) + getNode().registerInternalFailure("NodeStatus publication failed"); +} + +void NodeStatusProvider::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_TRACE("NodeStatusProvider", "Publishing node status by timer"); + publishWithErrorHandling(); +} + +void NodeStatusProvider::handleGlobalDiscoveryRequest(const protocol::GlobalDiscoveryRequest&) +{ + UAVCAN_TRACE("NodeStatusProvider", "Got GlobalDiscoveryRequest"); + publishWithErrorHandling(); +} + +void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, + protocol::GetNodeInfo::Response& rsp) +{ + UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request"); + assert(isNodeInfoInitialized()); + rsp = node_info_; +} + +int NodeStatusProvider::startAndPublish() +{ + int res = -1; + + if (!isNodeInfoInitialized()) + { + UAVCAN_TRACE("NodeStatusProvider", "Node info was not initialized"); + return -1; + } + + res = publish(); // Initial broadcast + if (res < 0) + goto fail; + + res = gdr_sub_.start(GlobalDiscoveryRequestCallback(this, &NodeStatusProvider::handleGlobalDiscoveryRequest)); + if (res < 0) + goto fail; + + res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest)); + if (res < 0) + goto fail; + + Timer::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS)); + + return res; + + fail: + assert(res < 0); + gdr_sub_.stop(); + gni_srv_.stop(); + Timer::stop(); + return res; +} + +void NodeStatusProvider::setStatusCode(uint8_t code) +{ + node_info_.status.status_code = code; +} + +void NodeStatusProvider::setName(const char* name) +{ + node_info_.name = name; +} + +void NodeStatusProvider::setSoftwareVersion(const protocol::SoftwareVersion& version) +{ + node_info_.software_version = version; +} + +void NodeStatusProvider::setHardwareVersion(const protocol::HardwareVersion& version) +{ + node_info_.hardware_version = version; +} + +} diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp new file mode 100644 index 0000000000..043e04b23c --- /dev/null +++ b/libuavcan/test/protocol/helpers.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include "../node/test_node.hpp" + + +template +class SubscriptionCollector : uavcan::Noncopyable +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructType; + + void handler(const ReceivedDataStructType& msg) + { + this->msg.reset(new ReceivedDataStructType(msg)); + } + +public: + std::auto_ptr msg; + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &SubscriptionCollector::handler); } +}; + + +template +struct SubscriberWithCollector +{ + typedef SubscriptionCollector Collector; + typedef uavcan::Subscriber Subscriber; + + Collector collector; + Subscriber subscriber; + + SubscriberWithCollector(uavcan::INode& node) + : subscriber(node) + { } + + int start() { return subscriber.start(collector.bind()); } +}; + + +template +class ServiceCallResultCollector : uavcan::Noncopyable +{ + typedef uavcan::ServiceCallResult ResultType; + + void handler(const ResultType& result) + { + this->result.reset(new ResultType(result)); + } + +public: + std::auto_ptr result; + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); } +}; + + +template +struct ServiceClientWithCollector +{ + typedef ServiceCallResultCollector Collector; + typedef uavcan::ServiceClient ServiceClient; + + Collector collector; + ServiceClient client; + + ServiceClientWithCollector(uavcan::INode& node) + : client(node) + { } + + int call(uavcan::NodeID node_id, const typename DataType::Request& request) + { + client.setCallback(collector.bind()); + return client.call(node_id, request); + } +}; + diff --git a/libuavcan/test/protocol/node_status_provider.cpp b/libuavcan/test/protocol/node_status_provider.cpp new file mode 100644 index 0000000000..a4560aa319 --- /dev/null +++ b/libuavcan/test/protocol/node_status_provider.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(NodeStatusProvider, Basic) +{ + InterlinkedTestNodes nodes; + + uavcan::NodeStatusProvider nsp(nodes.a); + + /* + * Initialization + */ + uavcan::protocol::HardwareVersion hwver; + hwver.major = 3; + hwver.minor = 14; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 2; + swver.minor = 18; + swver.build = 0x600DF00D; + + nsp.setHardwareVersion(hwver); + nsp.setSoftwareVersion(swver); + + ASSERT_TRUE(nsp.getName().empty()); + nsp.setName("superluminal_communication_unit"); + ASSERT_STREQ("superluminal_communication_unit", nsp.getName().c_str()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_INITIALIZING, nsp.getStatusCode()); + nsp.setStatusOk(); + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_OK, nsp.getStatusCode()); + + // Will fail - types are not registered + uavcan::GlobalDataTypeRegistry::instance().reset(); + ASSERT_GT(0, nsp.startAndPublish()); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + ASSERT_LE(0, nsp.startAndPublish()); + + /* + * Initial status publication + */ + SubscriberWithCollector status_sub(nodes.b); + + ASSERT_LE(0, status_sub.start()); + ASSERT_FALSE(status_sub.collector.msg.get()); // No data yet + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(status_sub.collector.msg.get()); // Was published at startup + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_OK, status_sub.collector.msg->status_code); + ASSERT_GE(1, status_sub.collector.msg->uptime_sec); + + /* + * Explicit node info request + */ + ServiceClientWithCollector gni_cln(nodes.b); + + nsp.setStatusCritical(); + + ASSERT_FALSE(gni_cln.collector.result.get()); // No data yet + ASSERT_LE(0, gni_cln.call(1, uavcan::protocol::GetNodeInfo::Request())); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(gni_cln.collector.result.get()); // Response must have been delivered + + ASSERT_TRUE(gni_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gni_cln.collector.result->server_node_id.get()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, gni_cln.collector.result->response.status.status_code); + + ASSERT_TRUE(hwver == gni_cln.collector.result->response.hardware_version); + ASSERT_TRUE(swver == gni_cln.collector.result->response.software_version); + + ASSERT_EQ(UAVCAN_VERSION_MAJOR, gni_cln.collector.result->response.uavcan_version.major); + ASSERT_EQ(UAVCAN_VERSION_MINOR, gni_cln.collector.result->response.uavcan_version.minor); + ASSERT_EQ(UAVCAN_VERSION_BUILD, gni_cln.collector.result->response.uavcan_version.build); + + ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->response.name); + + /* + * GlobalDiscoveryRequest + */ + uavcan::Publisher gdr_pub(nodes.b); + + status_sub.collector.msg.reset(); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_FALSE(status_sub.collector.msg.get()); // Nothing! + + ASSERT_LE(0, gdr_pub.broadcast(uavcan::protocol::GlobalDiscoveryRequest())); + + nsp.setStatusWarning(); + + status_sub.collector.msg.reset(); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(status_sub.collector.msg.get()); + ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_WARNING, status_sub.collector.msg->status_code); +}