Remote node reconfiguration

#include <iostream>
#include <unistd.h>
#include <vector>
#include <uavcan/uavcan.hpp>

/*
 * Remote reconfiguration services.
 */
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>

extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();

constexpr unsigned NodeMemoryPoolSize = 16384;

/**
 * Convenience function for blocking service calls.
 */
template <typename T>
std::pair<int, typename T::Response> performBlockingServiceCall(uavcan::INode& node,
                                                                uavcan::NodeID remote_node_id,
                                                                const typename T::Request& request)
{
    bool success = false;
    typename T::Response response;  // Generated types have zero-initializing constructors, always.

    uavcan::ServiceClient<T> client(node);
    client.setCallback([&](const uavcan::ServiceCallResult<T>& result)
        {
            success = result.isSuccessful();
            response = result.getResponse();
        });

    const int call_res = client.call(remote_node_id, request);
    if (call_res >= 0)
    {
        while (client.hasPendingCalls())
        {
            const int spin_res = node.spin(uavcan::MonotonicDuration::fromMSec(2));
            if (spin_res < 0)
            {
                return {spin_res, response};
            }
        }
        return {success ? 0 : -uavcan::ErrFailure, response};
    }
    return {call_res, response};
}

int main(int argc, const char** argv)
{
    if (argc < 3)
    {
        std::cerr << "Usage: " << argv[0] << " <node-id> <remote-node-id>" << std::endl;
        return 1;
    }

    const uavcan::NodeID self_node_id = std::stoi(argv[1]);
    const uavcan::NodeID remote_node_id = std::stoi(argv[2]);

    uavcan::Node<NodeMemoryPoolSize> node(getCanDriver(), getSystemClock());
    node.setNodeID(self_node_id);
    node.setName("org.uavcan.tutorial.configurator");

    const int node_start_res = node.start();
    if (node_start_res < 0)
    {
        throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));
    }

    node.setModeOperational();

    /*
     * Reading all params from the remote node (access by index); printing request/response
     * to stdout in YAML format.
     * Note that access by index should be used only to list params, not to get or set them.
     */
    std::vector<uavcan::protocol::param::GetSet::Response> remote_params;

    while (true)
    {
        uavcan::protocol::param::GetSet::Request req;
        req.index = remote_params.size();
        std::cout << "Param GET request:\n" << req << std::endl << std::endl;
        auto res = performBlockingServiceCall<uavcan::protocol::param::GetSet>(node, remote_node_id, req);
        if (res.first < 0)
        {
            throw std::runtime_error("Failed to get param: " + std::to_string(res.first));
        }
        if (res.second.name.empty())  // Empty name means no such param, which means we're finished
        {
            std::cout << "Param read done!\n\n" << std::endl;
            break;
        }
        std::cout << "Response:\n" << res.second << std::endl << std::endl;
        remote_params.push_back(res.second);
    }

    /*
     * Setting all parameters to their maximum values, if applicable. Access by name.
     */
    for (auto p : remote_params)
    {
        if (p.max_value.is(uavcan::protocol::param::NumericValue::Tag::empty))
        {
            std::cout << "Maximum value for parameter '" << p.name.c_str() << "' is not defined." << std::endl;
            continue;
        }

        uavcan::protocol::param::GetSet::Request req;
        req.name = p.name;

        if (p.max_value.is(uavcan::protocol::param::NumericValue::Tag::integer_value))
        {
            req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() =
                p.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
        }
        else if (p.max_value.is(uavcan::protocol::param::NumericValue::Tag::real_value))
        {
            req.value.to<uavcan::protocol::param::Value::Tag::real_value>() =
                p.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
        }
        else
        {
            throw std::runtime_error("Unknown tag in NumericValue: " + std::to_string(p.max_value.getTag()));
        }

        std::cout << "Param SET request:\n" << req << std::endl << std::endl;
        auto res = performBlockingServiceCall<uavcan::protocol::param::GetSet>(node, remote_node_id, req);
        if (res.first < 0)
        {
            throw std::runtime_error("Failed to set param: " + std::to_string(res.first));
        }
        std::cout << "Response:\n" << res.second << std::endl << std::endl;
    }
    std::cout << "Param set done!\n\n" << std::endl;

    /*
     * Reset all params back to their default values.
     */
    uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
    opcode_req.opcode = opcode_req.OPCODE_ERASE;
    auto erase_res =
        performBlockingServiceCall<uavcan::protocol::param::ExecuteOpcode>(node, remote_node_id, opcode_req);
    if (erase_res.first < 0)
    {
        throw std::runtime_error("Failed to erase params: " + std::to_string(erase_res.first));
    }
    std::cout << "Param erase response:\n" << erase_res.second << std::endl << std::endl;
    std::cout << "Param erase done!\n\n" << std::endl;

    /*
     * Restart the remote node.
     */
    uavcan::protocol::RestartNode::Request restart_req;
    restart_req.magic_number = restart_req.MAGIC_NUMBER;
    auto restart_res = performBlockingServiceCall<uavcan::protocol::RestartNode>(node, remote_node_id, restart_req);
    if (restart_res.first < 0)
    {
        throw std::runtime_error("Failed to restart: " + std::to_string(restart_res.first));
    }
    std::cout << "Restart response:\n" << restart_res.second << std::endl << std::endl;
    std::cout << "Restart done!" << std::endl;

    return 0;
}

#include <iostream>
#include <cstdlib>
#include <unistd.h>
#include <uavcan/uavcan.hpp>

/*
 * Implementations for the standard application-level functions are located in uavcan/protocol/.
 * The same path also contains the standard data types uavcan.protocol.*.
 */
#include <uavcan/protocol/param_server.hpp>

extern uavcan::ICanDriver& getCanDriver();
extern uavcan::ISystemClock& getSystemClock();

constexpr unsigned NodeMemoryPoolSize = 16384;

int main(int argc, const char** argv)
{
    if (argc < 2)
    {
        std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;
        return 1;
    }

    const int self_node_id = std::stoi(argv[1]);

    uavcan::Node<NodeMemoryPoolSize> node(getCanDriver(), getSystemClock());
    node.setNodeID(self_node_id);
    node.setName("org.uavcan.tutorial.configuree");

    const int node_start_res = node.start();
    if (node_start_res < 0)
    {
        throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));
    }

    /*
     * This would be our configuration storage.
     */
    static struct Params
    {
        unsigned foo = 42;
        float bar = 0.123456F;
        double baz = 1e-5;
        std::string booz = "Hello world!";
    } configuration;

    /*
     * Now, we need to define some glue logic between the server (below) and our configuration storage (above).
     * This is done via the interface uavcan::IParamManager.
     */
    class : public uavcan::IParamManager
    {
        void getParamNameByIndex(Index index, Name& out_name) const override
        {
            if (index == 0) { out_name = "foo"; }
            if (index == 1) { out_name = "bar"; }
            if (index == 2) { out_name = "baz"; }
            if (index == 3) { out_name = "booz"; }
        }

        void assignParamValue(const Name& name, const Value& value) override
        {
            if (name == "foo")
            {
                /*
                 * Parameter "foo" is an integer, so we accept only integer values here.
                 */
                if (value.is(uavcan::protocol::param::Value::Tag::integer_value))
                {
                    configuration.foo = *value.as<uavcan::protocol::param::Value::Tag::integer_value>();
                }
            }
            else if (name == "bar")
            {
                /*
                 * Parameter "bar" is a floating point, so we accept only float values here.
                 */
                if (value.is(uavcan::protocol::param::Value::Tag::real_value))
                {
                    configuration.bar = *value.as<uavcan::protocol::param::Value::Tag::real_value>();
                }
            }
            else if (name == "baz")
            {
                /*
                 * Ditto
                 */
                if (value.is(uavcan::protocol::param::Value::Tag::real_value))
                {
                    configuration.baz = *value.as<uavcan::protocol::param::Value::Tag::real_value>();
                }
            }
            else if (name == "booz")
            {
                /*
                 * Parameter "booz" is a string, so we take only strings.
                 */
                if (value.is(uavcan::protocol::param::Value::Tag::string_value))
                {
                    configuration.booz = value.as<uavcan::protocol::param::Value::Tag::string_value>()->c_str();
                }
            }
            else
            {
                std::cout << "Can't assign parameter: " << name.c_str() << std::endl;
            }
        }

        void readParamValue(const Name& name, Value& out_value) const override
        {
            if (name == "foo")
            {
                out_value.to<uavcan::protocol::param::Value::Tag::integer_value>() = configuration.foo;
            }
            else if (name == "bar")
            {
                out_value.to<uavcan::protocol::param::Value::Tag::real_value>() = configuration.bar;
            }
            else if (name == "baz")
            {
                out_value.to<uavcan::protocol::param::Value::Tag::real_value>() = configuration.baz;
            }
            else if (name == "booz")
            {
                out_value.to<uavcan::protocol::param::Value::Tag::string_value>() = configuration.booz.c_str();
            }
            else
            {
                std::cout << "Can't read parameter: " << name.c_str() << std::endl;
            }
        }

        int saveAllParams() override
        {
            std::cout << "Save - this implementation does not require any action" << std::endl;
            return 0;     // Zero means that everything is fine.
        }

        int eraseAllParams() override
        {
            std::cout << "Erase - all params reset to default values" << std::endl;
            configuration = Params();
            return 0;
        }

        /**
         * Note that this method is optional. It can be left unimplemented.
         */
        void readParamDefaultMaxMin(const Name& name, Value& out_def,
                                    NumericValue& out_max, NumericValue& out_min) const override
        {
            if (name == "foo")
            {
                out_def.to<uavcan::protocol::param::Value::Tag::integer_value>() = Params().foo;
                out_max.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = 9000;
                out_min.to<uavcan::protocol::param::NumericValue::Tag::integer_value>() = 0;
            }
            else if (name == "bar")
            {
                out_def.to<uavcan::protocol::param::Value::Tag::real_value>() = Params().bar;
                out_max.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 1;
                out_min.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 0;
            }
            else if (name == "baz")
            {
                out_def.to<uavcan::protocol::param::Value::Tag::real_value>() = Params().baz;
                out_max.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 1;
                out_min.to<uavcan::protocol::param::NumericValue::Tag::real_value>() = 0;
            }
            else if (name == "booz")
            {
                out_def.to<uavcan::protocol::param::Value::Tag::string_value>() = Params().booz.c_str();
                std::cout << "Limits for 'booz' are not defined" << std::endl;
            }
            else
            {
                std::cout << "Can't read the limits for parameter: " << name.c_str() << std::endl;
            }
        }
    } param_manager;

    /*
     * Initializing the configuration server.
     * A pointer to the glue logic object is passed to the method start().
     */
    uavcan::ParamServer server(node);

    const int server_start_res = server.start(&param_manager);
    if (server_start_res < 0)
    {
        throw std::runtime_error("Failed to start ParamServer: " + std::to_string(server_start_res));
    }

    /*
     * Now, this node can be reconfigured via UAVCAN. Awesome.
     * Many embedded applications require a restart before the new configuration settings can
     * be applied, so it is highly recommended to also support the remote restart service.
     */
    class : public uavcan::IRestartRequestHandler
    {
        bool handleRestartRequest(uavcan::NodeID request_source) override
        {
            std::cout << "Got a remote restart request from " << int(request_source.get()) << std::endl;
            /*
             * We won't really restart, so return 'false'.
             * Returning 'true' would mean that we're going to restart for real.
             * Note that it is recognized that some nodes may not be able to respond to the
             * restart request (e.g. if they restart immediately from the service callback).
             */
            return false;
        }
    } restart_request_handler;

    /*
     * Installing the restart request handler.
     */
    node.setRestartRequestHandler(&restart_request_handler); // Done.

    /*
     * Running the node.
     */
    node.setModeOperational();
    while (true)
    {
        const int res = node.spin(uavcan::MonotonicDuration::getInfinite());
        if (res < 0)
        {
            std::cerr << "Transient failure: " << res << std::endl;
        }
    }
}