RestartRequestServer

libuavcan/include/uavcan/protocol/restart_request_server.hpp

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/protocol/RestartNode.hpp>
+
+namespace uavcan
+{
+
+class IRestartRequestHandler
+{
+public:
+    virtual ~IRestartRequestHandler() { }
+    virtual bool handleRestartRequest(NodeID request_source) = 0;
+};
+
+
+class RestartRequestServer : Noncopyable
+{
+    typedef MethodBinder<const RestartRequestServer*,
+        void(RestartRequestServer::*)(const ReceivedDataStructure<protocol::RestartNode::Request>&,
+            protocol::RestartNode::Response&) const> RestartNodeCallback;
+
+    ServiceServer<protocol::RestartNode, RestartNodeCallback> srv_;
+    IRestartRequestHandler* handler_;
+
+    void handleRestartNode(const ReceivedDataStructure<protocol::RestartNode::Request>& request,
+                           protocol::RestartNode::Response& response) const;
+
+public:
+    RestartRequestServer(INode& node)
+    : srv_(node)
+    , handler_(NULL)
+    { }
+
+    IRestartRequestHandler* getHandler() const { return handler_; }
+    void setHandler(IRestartRequestHandler* handler) { handler_ = handler; }
+
+    int start();
+};
+
+}

libuavcan/src/protocol/restart_request_server.cpp

@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/protocol/restart_request_server.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+
+void RestartRequestServer::handleRestartNode(const ReceivedDataStructure<protocol::RestartNode::Request>& request,
+                                             protocol::RestartNode::Response& response) const
+{
+    UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get()));
+    response.ok = false;
+    if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER)
+    {
+        if (handler_)
+        {
+            response.ok = handler_->handleRestartRequest(request.getSrcNodeID());
+        }
+        UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected"));
+    }
+    else
+    {
+        UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx",
+                     static_cast<unsigned long long>(request.magic_number));
+    }
+}
+
+int RestartRequestServer::start()
+{
+    return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode));
+}
+
+}

libuavcan/test/protocol/restart_request_server.cpp

@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/restart_request_server.hpp>
+#include "helpers.hpp"
+
+
+struct Handler : public uavcan::IRestartRequestHandler
+{
+    bool accept;
+
+    bool handleRestartRequest(uavcan::NodeID request_source)
+    {
+        std::cout << "Restart request from " << int(request_source.get()) << " will be "
+            << (accept ? "accepted" : "rejected") << std::endl;
+        return accept;
+    }
+};
+
+
+TEST(RestartRequestServer, Basic)
+{
+    InterlinkedTestNodes nodes;
+
+    uavcan::RestartRequestServer rrs(nodes.a);
+
+    ServiceClientWithCollector<uavcan::protocol::RestartNode> rrs_cln(nodes.b);
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::RestartNode> _reg1;
+
+    ASSERT_LE(0, rrs.start());
+
+    uavcan::protocol::RestartNode::Request request;
+    request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER;
+
+    /*
+     * Rejected - handler was not set
+     */
+    ASSERT_LE(0, rrs_cln.call(1, request));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2));
+
+    ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+
+    /*
+     * Accepted
+     */
+    Handler handler;
+    handler.accept = true;
+    rrs.setHandler(&handler);
+
+    ASSERT_LE(0, rrs_cln.call(1, request));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2));
+
+    ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+    ASSERT_TRUE(rrs_cln.collector.result->response.ok);
+
+    /*
+     * Rejected by handler
+     */
+    handler.accept = false;
+
+    ASSERT_LE(0, rrs_cln.call(1, request));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2));
+
+    ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+
+    /*
+     * Rejected because of invalid magic number
+     */
+    handler.accept = true;
+
+    ASSERT_LE(0, rrs_cln.call(1, uavcan::protocol::RestartNode::Request()));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2));
+
+    ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
+    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+}