The work on the highest-level concepts have just started; here goes NodeStatusProvider with tests

libuavcan/include/uavcan/protocol/node_status_provider.hpp

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/impl_constants.hpp>
+#include <uavcan/protocol/NodeStatus.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+#include <uavcan/protocol/GlobalDiscoveryRequest.hpp>
+
+namespace uavcan
+{
+
+class NodeStatusProvider : private Timer
+{
+    typedef MethodBinder<NodeStatusProvider*, void(NodeStatusProvider::*)(const protocol::GlobalDiscoveryRequest&)>
+        GlobalDiscoveryRequestCallback;
+    typedef MethodBinder<NodeStatusProvider*,
+        void(NodeStatusProvider::*)(const protocol::GetNodeInfo::Request&,
+                                    protocol::GetNodeInfo::Response&)> GetNodeInfoCallback;
+
+    const MonotonicTime creation_timestamp_;
+
+    Publisher<protocol::NodeStatus> node_status_pub_;
+    Subscriber<protocol::GlobalDiscoveryRequest, GlobalDiscoveryRequestCallback> gdr_sub_;
+    ServiceServer<protocol::GetNodeInfo, GetNodeInfoCallback> 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);
+};
+
+}

libuavcan/src/protocol/node_status_provider.cpp

@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+
+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;
+}
+
+}

libuavcan/test/protocol/helpers.hpp

@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include "../node/test_node.hpp"
+
+
+template <typename DataType>
+class SubscriptionCollector : uavcan::Noncopyable
+{
+    typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructType;
+
+    void handler(const ReceivedDataStructType& msg)
+    {
+        this->msg.reset(new ReceivedDataStructType(msg));
+    }
+
+public:
+    std::auto_ptr<ReceivedDataStructType> msg;
+
+    typedef uavcan::MethodBinder<SubscriptionCollector*,
+        void (SubscriptionCollector::*)(const ReceivedDataStructType&)> Binder;
+
+    Binder bind() { return Binder(this, &SubscriptionCollector::handler); }
+};
+
+
+template <typename DataType>
+struct SubscriberWithCollector
+{
+    typedef SubscriptionCollector<DataType> Collector;
+    typedef uavcan::Subscriber<DataType, typename Collector::Binder> Subscriber;
+
+    Collector collector;
+    Subscriber subscriber;
+
+    SubscriberWithCollector(uavcan::INode& node)
+    : subscriber(node)
+    { }
+
+    int start() { return subscriber.start(collector.bind()); }
+};
+
+
+template <typename DataType>
+class ServiceCallResultCollector : uavcan::Noncopyable
+{
+    typedef uavcan::ServiceCallResult<DataType> ResultType;
+
+    void handler(const ResultType& result)
+    {
+        this->result.reset(new ResultType(result));
+    }
+
+public:
+    std::auto_ptr<ResultType> result;
+
+    typedef uavcan::MethodBinder<ServiceCallResultCollector*,
+        void (ServiceCallResultCollector::*)(const ResultType&)> Binder;
+
+    Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); }
+};
+
+
+template <typename DataType>
+struct ServiceClientWithCollector
+{
+    typedef ServiceCallResultCollector<DataType> Collector;
+    typedef uavcan::ServiceClient<DataType, typename Collector::Binder> 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);
+    }
+};
+

libuavcan/test/protocol/node_status_provider.cpp

@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_status_provider.hpp>
+#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<uavcan::protocol::NodeStatus> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg3;
+    ASSERT_LE(0, nsp.startAndPublish());
+
+    /*
+     * Initial status publication
+     */
+    SubscriberWithCollector<uavcan::protocol::NodeStatus> 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<uavcan::protocol::GetNodeInfo> 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<uavcan::protocol::GlobalDiscoveryRequest> 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);
+}