<commit_before>#pragma once

#include <type_traits>
#include <algorithm>
#include <stdexcept>
#include <cstddef>
#include <utility>
#include <string>
#include <vector>
#include <uv.h>

namespace uvw {

namespace details {

enum class UVHandleType: std::underlying_type_t<uv_handle_type> {
    UNKNOWN = UV_UNKNOWN_HANDLE,
    ASYNC = UV_ASYNC,
    CHECK = UV_CHECK,
    FS_EVENT = UV_FS_EVENT,
    FS_POLL = UV_FS_POLL,
    HANDLE = UV_HANDLE,
    IDLE = UV_IDLE,
    PIPE = UV_NAMED_PIPE,
    POLL = UV_POLL,
    PREPARE = UV_PREPARE,
    PROCESS = UV_PROCESS,
    STREAM = UV_STREAM,
    TCP = UV_TCP,
    TIMER = UV_TIMER,
    TTY = UV_TTY,
    UDP = UV_UDP,
    SIGNAL = UV_SIGNAL,
    FILE = UV_FILE
};

template<typename T>
struct UVTypeWrapper {
    using Type = T;

    constexpr UVTypeWrapper(Type val): value{val} { }
    constexpr operator Type() const noexcept { return value; }

private:
    const Type value;
};

}

/**
 * @brief Utility class to handle flags.
 *
 * This class can be used to handle flags of a same enumeration type.

 * It is meant to be used as an argument for functions and member methods and
 * as part of events.

 * `Flags<E>` objects can be easily _or-ed_ and _and-ed_ with other instances of
 * the same type or with instances of the type `E` (that is, the actual flag
 * type), thus converted to the underlying type when needed.
 */
template<typename E>
class Flags final {
    using InnerType = std::underlying_type_t<E>;

    constexpr InnerType toInnerType(E flag) const noexcept { return static_cast<InnerType>(flag); }

public:
    using Type = InnerType;

    /**
     * @brief Constructs a Flags object from a value of the enum `E`.
     * @param flag An value of the enum `E`.
     */
    constexpr Flags(E flag) noexcept: flags{toInnerType(flag)} { }

    /**
     * @brief Constructs a Flags object from an instance of the underlying type
     * of the enum `E`.
     * @param f An instance of the underlying type of the enum `E`.
     */
    constexpr Flags(Type f): flags{f} { }

    /**
     * @brief Constructs an uninitialized Flags object.
     */
    constexpr Flags(): flags{} { }

    constexpr Flags(const Flags &f) noexcept: flags{f.flags} {  }
    constexpr Flags(Flags &&f) noexcept: flags{std::move(f.flags)} {  }

    ~Flags() noexcept { static_assert(std::is_enum<E>::value, "!"); }

    constexpr Flags& operator=(const Flags &f) noexcept {
        flags = f.flags;
        return *this;
    }

    constexpr Flags& operator=(Flags &&f) noexcept {
        flags = std::move(f.flags);
        return *this;
    }

    /**
     * @brief Or operator.
     * @param f A valid instance of Flags.
     * @return This instance _or-ed_ with `f`.
     */
    constexpr Flags operator|(const Flags &f) const noexcept { return Flags(flags | f.flags); }

    /**
     * @brief Or operator.
     * @param flag A value of the enum `E`.
     * @return This instance _or-ed_ with `flag`.
     */
    constexpr Flags operator|(E flag) const noexcept { return Flags(flags | toInnerType(flag)); }

    /**
     * @brief And operator.
     * @param f A valid instance of Flags.
     * @return This instance _and-ed_ with `f`.
     */
    constexpr Flags operator&(const Flags &f) const noexcept { return Flags(flags & f.flags); }

    /**
     * @brief And operator.
     * @param flag A value of the enum `E`.
     * @return This instance _and-ed_ with `flag`.
     */
    constexpr Flags operator&(E flag) const noexcept { return Flags(flags & toInnerType(flag)); }

    /**
     * @brief Checks if this instance is initialized.
     * @return False if it's uninitialized, true otherwise.
     */
    explicit constexpr operator bool() const noexcept { return !(flags == InnerType{}); }

    /**
     * @brief Casts the instance to the underlying type of `E`.
     * @return An integral representation of the contained flags.
     */
    constexpr operator Type() const noexcept { return flags; }

private:
    InnerType flags;
};

/**
 * @brief Windows size representation.
 */
struct WinSize {
    int width; /*!< The _width_ of the given window. */
    int height; /*!< The _height_ of the given window. */
};

using HandleType = details::UVHandleType;

using FileHandle = details::UVTypeWrapper<uv_file>;
using OSSocketHandle = details::UVTypeWrapper<uv_os_sock_t>;
using OSFileDescriptor = details::UVTypeWrapper<uv_os_fd_t>;

using TimeSpec = uv_timespec_t;
using Stat = uv_stat_t;
using Uid = uv_uid_t;
using Gid = uv_gid_t;

/**
 * @brief The IPv4 tag.
 *
 * To be used as template parameter to switch between IPv4 and IPv6.
 */
struct IPv4 { };

/**
 * @brief The IPv6 tag.
 *
 * To be used as template parameter to switch between IPv4 and IPv6.
 */
struct IPv6 { };

/**
 * @brief Address representation.
 */
struct Addr {
    std::string ip; /*!< Either an IPv4 or an IPv6. */
    unsigned int port; /*!< A valid service identifier. */
};

/**
 * \brief CPU information.
 */
struct CPUInfo {
    using CPUTime = decltype(uv_cpu_info_t::cpu_times);

    std::string model; /*!< The model of the CPU. */
    int speed; /*!< The frequency of the CPU. */

    /**
     * @brief CPU times.
     *
     * It is built up of the following data members: `user`, `nice`, `sys`,
     * `idle`, `irq`, all of them having type `uint64_t`.
     */
    CPUTime times;
};

/**
 * \brief Interface address.
 */
struct InterfaceAddress {
    std::string name; /*!< The name of the interface (as an example _eth0_). */
    std::string physical; /*!< The physical address. */
    bool internal; /*!< True if it is an internal interface (as an example _loopback_), false otherwise. */
    Addr address; /*!< The address of the given interface. */
    Addr netmask; /*!< The netmask of the given interface. */
};

namespace details {

template<typename>
struct IpTraits;

template<>
struct IpTraits<IPv4> {
    using Type = sockaddr_in;
    using AddrFuncType = int(*)(const char *, int, Type *);
    using NameFuncType = int(*)(const Type *, char *, std::size_t);
    static constexpr AddrFuncType addrFunc = &uv_ip4_addr;
    static constexpr NameFuncType nameFunc = &uv_ip4_name;
    static constexpr auto sinPort(const Type *addr) { return addr->sin_port; }
};

template<>
struct IpTraits<IPv6> {
    using Type = sockaddr_in6;
    using AddrFuncType = int(*)(const char *, int, Type *);
    using NameFuncType = int(*)(const Type *, char *, std::size_t);
    static constexpr AddrFuncType addrFunc = &uv_ip6_addr;
    static constexpr NameFuncType nameFunc = &uv_ip6_name;
    static constexpr auto sinPort(const Type *addr) { return addr->sin6_port; }
};

template<typename I, typename..., std::size_t N = 128>
Addr address(const typename details::IpTraits::Type *aptr) noexcept {
    Addr addr;
    char name[N];

    int err = details::IpTraits::nameFunc(aptr, name, N);

    if(0 == err) {
        addr.port = ntohs(details::IpTraits::sinPort(aptr));
        addr.ip = std::string{name};
    }

    return addr;
}

template<typename I, typename F, typename H>
Addr address(F &&f, const H *handle) noexcept {
    sockaddr_storage ssto;
    int len = sizeof(ssto);
    Addr addr{};

    int err = std::forward<F>(f)(handle, reinterpret_cast<sockaddr *>(&ssto), &len);

    if(0 == err) {
        typename IpTraits::Type *aptr = reinterpret_cast<typename IpTraits::Type *>(&ssto);
        addr = address(aptr);
    }

    return addr;
}

template<typename F, typename H, typename..., std::size_t N = 128>
std::string path(F &&f, H *handle) noexcept {
    std::size_t size = N;
    char buf[size];
    std::string str{};
    auto err = std::forward<F>(f)(handle, buf, &size);

    if(UV_ENOBUFS == err) {
        std::unique_ptr<char[]> data{new char[size]};
        err = std::forward<F>(f)(handle, data.get(), &size);

        if(0 == err) {
            str = data.get();
        }
    } else {
        str.assign(buf, size);
    }

    return str;
}

}

/**
 * @brief Miscellaneous utilities.
 *
 * Miscellaneous functions that don’t really belong to any other class.
 */
struct Utilities {
    using MallocFuncType = void*(*)(size_t);
    using ReallocFuncType = void*(*)(void*, size_t);
    using CallocFuncType = void*(*)(size_t, size_t);
    using FreeFuncType = void(*)(void*);

    /**
     * @brief Gets the type of the stream to be used with the given descriptor.
     *
     * Returns the type of stream that should be used with a given file
     * descriptor.

     * Usually this will be used during initialization to guess the type of the
     * stdio streams.
     *
     * @param file A valid descriptor.
     * @return One of the following types:
     *
     * * `HandleType::UNKNOWN`
     * * `HandleType::PIPE`
     * * `HandleType::TCP`
     * * `HandleType::TTY`
     * * `HandleType::UDP`
     * * `HandleType::FILE`
     */
    static HandleType guessHandle(FileHandle file) {
        auto type = uv_guess_handle(file);

        switch(type) {
        case UV_ASYNC:
            return HandleType::ASYNC;
        case UV_CHECK:
            return HandleType::CHECK;
        case UV_FS_EVENT:
            return HandleType::FS_EVENT;
        case UV_FS_POLL:
            return HandleType::FS_POLL;
        case UV_HANDLE:
            return HandleType::HANDLE;
        case UV_IDLE:
            return HandleType::IDLE;
        case UV_NAMED_PIPE:
            return HandleType::PIPE;
        case UV_POLL:
            return HandleType::POLL;
        case UV_PREPARE:
            return HandleType::PREPARE;
        case UV_PROCESS:
            return HandleType::PROCESS;
        case UV_STREAM:
            return HandleType::STREAM;
        case UV_TCP:
            return HandleType::TCP;
        case UV_TIMER:
            return HandleType::TIMER;
        case UV_TTY:
            return HandleType::TTY;
        case UV_UDP:
            return HandleType::UDP;
        case UV_SIGNAL:
            return HandleType::SIGNAL;
        case UV_FILE:
            return HandleType::FILE;
        default:
            return HandleType::UNKNOWN;
        }
    }

    /** @brief Gets information about the CPUs on the system.
     *
     * This function can be used to query the underlying system and get a set of
     * descriptors of all the available CPUs.
     *
     * @return A set of descriptors of all the available CPUs.
     */
    static std::vector<CPUInfo> cpuInfo() noexcept {
        std::vector<CPUInfo> cpuinfos;

        uv_cpu_info_t *infos;
        int count;

        if(0 == uv_cpu_info(&infos, &count)) {
            std::for_each(infos, infos+count, [&cpuinfos](const auto &info) {
                CPUInfo cpuinfo;

                cpuinfo.model = info.model;
                cpuinfo.speed = info.speed;
                cpuinfo.times = info.cpu_times;

                cpuinfos.push_back(std::move(cpuinfo));
            });

            uv_free_cpu_info(infos, count);
        }

        return cpuinfos;
    }

    /**
     * @brief Gets a set of descriptors of all the available interfaces.
     *
     * This function can be used to query the underlying system and get a set of
     * descriptors of all the available interfaces, either internal or not.
     *
     * @return A set of descriptors of all the available interfaces.
     */
    static std::vector<InterfaceAddress> interfaceAddresses() noexcept {
        std::vector<InterfaceAddress> interfaces;

        uv_interface_address_t *ifaces;
        int count;

        if(0 == uv_interface_addresses(&ifaces, &count)) {
            std::for_each(ifaces, ifaces+count, [&interfaces](const auto &iface) {
                InterfaceAddress interface;

                interface.name = iface.name;
                interface.physical = iface.phys_addr;
                interface.internal = iface.is_internal;

                if(iface.address.address4.sin_family == AF_INET) {
                    interface.address = details::address<IPv4>(&iface.address.address4);
                    interface.netmask = details::address<IPv4>(&iface.netmask.netmask4);
                } else if(iface.address.address4.sin_family == AF_INET6) {
                    interface.address = details::address<IPv6>(&iface.address.address6);
                    interface.netmask = details::address<IPv6>(&iface.netmask.netmask6);
                }

                interfaces.push_back(std::move(interface));
            });

            uv_free_interface_addresses(ifaces, count);
        }

        return interfaces;
    }

    /**
     * @brief Override the use of some standard library’s functions.
     *
     * Override the use of the standard library’s memory allocation
     * functions.

     * This method must be invoked before any other `uvw` function is called or
     * after all resources have been freed and thus the underlying library
     * doesn’t reference any allocated memory chunk.
     *
     * If any of the function pointers is _null_, the invokation will fail.
     *
     * **Note**: there is no protection against changing the allocator multiple
     * times. If the user changes it they are responsible for making sure the
     * allocator is changed while no memory was allocated with the previous
     * allocator, or that they are compatible.
     *
     * @param mallocFunc Replacement function for _malloc_.
     * @param reallocFunc Replacement function for _realloc_.
     * @param callocFunc Replacement function for _calloc_.
     * @param freeFunc Replacement function for _free_.
     * @return True in case of success, false otherwise.
     */
    static bool replaceAllocator(MallocFuncType mallocFunc, ReallocFuncType reallocFunc, CallocFuncType callocFunc, FreeFuncType freeFunc) {
        return (0 == uv_replace_allocator(mallocFunc, reallocFunc, callocFunc, freeFunc));
    }
};

/**
 * TODO
 *
 * * uv_uptime
 * * uv_getrusage
 * * uv_cpu_info
 * * uv_free_cpu_info
 * * uv_loadavg
 * * uv_exepath
 * * uv_cwd
 * * uv_chdir
 * * uv_os_homedir
 * * uv_os_tmpdir
 * * uv_os_get_passwd
 * * uv_os_free_passwd
 * * uv_get_total_memory
 * * uv_hrtime
 */

}
<commit_msg>clean up<commit_after>#pragma once

#include <type_traits>
#include <algorithm>
#include <stdexcept>
#include <cstddef>
#include <utility>
#include <string>
#include <vector>
#include <uv.h>

namespace uvw {

namespace details {

enum class UVHandleType: std::underlying_type_t<uv_handle_type> {
    UNKNOWN = UV_UNKNOWN_HANDLE,
    ASYNC = UV_ASYNC,
    CHECK = UV_CHECK,
    FS_EVENT = UV_FS_EVENT,
    FS_POLL = UV_FS_POLL,
    HANDLE = UV_HANDLE,
    IDLE = UV_IDLE,
    PIPE = UV_NAMED_PIPE,
    POLL = UV_POLL,
    PREPARE = UV_PREPARE,
    PROCESS = UV_PROCESS,
    STREAM = UV_STREAM,
    TCP = UV_TCP,
    TIMER = UV_TIMER,
    TTY = UV_TTY,
    UDP = UV_UDP,
    SIGNAL = UV_SIGNAL,
    FILE = UV_FILE
};

template<typename T>
struct UVTypeWrapper {
    using Type = T;

    constexpr UVTypeWrapper(Type val): value{val} { }
    constexpr operator Type() const noexcept { return value; }

private:
    const Type value;
};

}

/**
 * @brief Utility class to handle flags.
 *
 * This class can be used to handle flags of a same enumeration type.

 * It is meant to be used as an argument for functions and member methods and
 * as part of events.

 * `Flags<E>` objects can be easily _or-ed_ and _and-ed_ with other instances of
 * the same type or with instances of the type `E` (that is, the actual flag
 * type), thus converted to the underlying type when needed.
 */
template<typename E>
class Flags final {
    using InnerType = std::underlying_type_t<E>;

    constexpr InnerType toInnerType(E flag) const noexcept { return static_cast<InnerType>(flag); }

public:
    using Type = InnerType;

    /**
     * @brief Constructs a Flags object from a value of the enum `E`.
     * @param flag An value of the enum `E`.
     */
    constexpr Flags(E flag) noexcept: flags{toInnerType(flag)} { }

    /**
     * @brief Constructs a Flags object from an instance of the underlying type
     * of the enum `E`.
     * @param f An instance of the underlying type of the enum `E`.
     */
    constexpr Flags(Type f): flags{f} { }

    /**
     * @brief Constructs an uninitialized Flags object.
     */
    constexpr Flags(): flags{} { }

    constexpr Flags(const Flags &f) noexcept: flags{f.flags} {  }
    constexpr Flags(Flags &&f) noexcept: flags{std::move(f.flags)} {  }

    ~Flags() noexcept { static_assert(std::is_enum<E>::value, "!"); }

    constexpr Flags& operator=(const Flags &f) noexcept {
        flags = f.flags;
        return *this;
    }

    constexpr Flags& operator=(Flags &&f) noexcept {
        flags = std::move(f.flags);
        return *this;
    }

    /**
     * @brief Or operator.
     * @param f A valid instance of Flags.
     * @return This instance _or-ed_ with `f`.
     */
    constexpr Flags operator|(const Flags &f) const noexcept { return Flags(flags | f.flags); }

    /**
     * @brief Or operator.
     * @param flag A value of the enum `E`.
     * @return This instance _or-ed_ with `flag`.
     */
    constexpr Flags operator|(E flag) const noexcept { return Flags(flags | toInnerType(flag)); }

    /**
     * @brief And operator.
     * @param f A valid instance of Flags.
     * @return This instance _and-ed_ with `f`.
     */
    constexpr Flags operator&(const Flags &f) const noexcept { return Flags(flags & f.flags); }

    /**
     * @brief And operator.
     * @param flag A value of the enum `E`.
     * @return This instance _and-ed_ with `flag`.
     */
    constexpr Flags operator&(E flag) const noexcept { return Flags(flags & toInnerType(flag)); }

    /**
     * @brief Checks if this instance is initialized.
     * @return False if it's uninitialized, true otherwise.
     */
    explicit constexpr operator bool() const noexcept { return !(flags == InnerType{}); }

    /**
     * @brief Casts the instance to the underlying type of `E`.
     * @return An integral representation of the contained flags.
     */
    constexpr operator Type() const noexcept { return flags; }

private:
    InnerType flags;
};

/**
 * @brief Windows size representation.
 */
struct WinSize {
    int width; /*!< The _width_ of the given window. */
    int height; /*!< The _height_ of the given window. */
};

using HandleType = details::UVHandleType;

using FileHandle = details::UVTypeWrapper<uv_file>;
using OSSocketHandle = details::UVTypeWrapper<uv_os_sock_t>;
using OSFileDescriptor = details::UVTypeWrapper<uv_os_fd_t>;

using TimeSpec = uv_timespec_t;
using Stat = uv_stat_t;
using Uid = uv_uid_t;
using Gid = uv_gid_t;

/**
 * @brief The IPv4 tag.
 *
 * To be used as template parameter to switch between IPv4 and IPv6.
 */
struct IPv4 { };

/**
 * @brief The IPv6 tag.
 *
 * To be used as template parameter to switch between IPv4 and IPv6.
 */
struct IPv6 { };

/**
 * @brief Address representation.
 */
struct Addr {
    std::string ip; /*!< Either an IPv4 or an IPv6. */
    unsigned int port; /*!< A valid service identifier. */
};

/**
 * \brief CPU information.
 */
struct CPUInfo {
    using CPUTime = decltype(uv_cpu_info_t::cpu_times);

    std::string model; /*!< The model of the CPU. */
    int speed; /*!< The frequency of the CPU. */

    /**
     * @brief CPU times.
     *
     * It is built up of the following data members: `user`, `nice`, `sys`,
     * `idle`, `irq`, all of them having type `uint64_t`.
     */
    CPUTime times;
};

/**
 * \brief Interface address.
 */
struct InterfaceAddress {
    std::string name; /*!< The name of the interface (as an example _eth0_). */
    std::string physical; /*!< The physical address. */
    bool internal; /*!< True if it is an internal interface (as an example _loopback_), false otherwise. */
    Addr address; /*!< The address of the given interface. */
    Addr netmask; /*!< The netmask of the given interface. */
};

namespace details {

template<typename>
struct IpTraits;

template<>
struct IpTraits<IPv4> {
    using Type = sockaddr_in;
    using AddrFuncType = int(*)(const char *, int, Type *);
    using NameFuncType = int(*)(const Type *, char *, std::size_t);
    static constexpr AddrFuncType addrFunc = &uv_ip4_addr;
    static constexpr NameFuncType nameFunc = &uv_ip4_name;
    static constexpr auto sinPort(const Type *addr) { return addr->sin_port; }
};

template<>
struct IpTraits<IPv6> {
    using Type = sockaddr_in6;
    using AddrFuncType = int(*)(const char *, int, Type *);
    using NameFuncType = int(*)(const Type *, char *, std::size_t);
    static constexpr AddrFuncType addrFunc = &uv_ip6_addr;
    static constexpr NameFuncType nameFunc = &uv_ip6_name;
    static constexpr auto sinPort(const Type *addr) { return addr->sin6_port; }
};

template<typename I, typename..., std::size_t N = 128>
Addr address(const typename details::IpTraits::Type *aptr) noexcept {
    Addr addr;
    char name[N];

    int err = details::IpTraits::nameFunc(aptr, name, N);

    if(0 == err) {
        addr.port = ntohs(details::IpTraits::sinPort(aptr));
        addr.ip = std::string{name};
    }

    return addr;
}

template<typename I, typename F, typename H>
Addr address(F &&f, const H *handle) noexcept {
    sockaddr_storage ssto;
    int len = sizeof(ssto);
    Addr addr{};

    int err = std::forward<F>(f)(handle, reinterpret_cast<sockaddr *>(&ssto), &len);

    if(0 == err) {
        typename IpTraits::Type *aptr = reinterpret_cast<typename IpTraits::Type *>(&ssto);
        addr = address(aptr);
    }

    return addr;
}

template<typename F, typename H, typename..., std::size_t N = 128>
std::string path(F &&f, H *handle) noexcept {
    std::size_t size = N;
    char buf[size];
    std::string str{};
    auto err = std::forward<F>(f)(handle, buf, &size);

    if(UV_ENOBUFS == err) {
        std::unique_ptr<char[]> data{new char[size]};
        err = std::forward<F>(f)(handle, data.get(), &size);

        if(0 == err) {
            str = data.get();
        }
    } else {
        str.assign(buf, size);
    }

    return str;
}

}

/**
 * @brief Miscellaneous utilities.
 *
 * Miscellaneous functions that don’t really belong to any other class.
 */
struct Utilities {
    using MallocFuncType = void*(*)(size_t);
    using ReallocFuncType = void*(*)(void*, size_t);
    using CallocFuncType = void*(*)(size_t, size_t);
    using FreeFuncType = void(*)(void*);

    /**
     * @brief Gets the type of the stream to be used with the given descriptor.
     *
     * Returns the type of stream that should be used with a given file
     * descriptor.

     * Usually this will be used during initialization to guess the type of the
     * stdio streams.
     *
     * @param file A valid descriptor.
     * @return One of the following types:
     *
     * * `HandleType::UNKNOWN`
     * * `HandleType::PIPE`
     * * `HandleType::TCP`
     * * `HandleType::TTY`
     * * `HandleType::UDP`
     * * `HandleType::FILE`
     */
    static HandleType guessHandle(FileHandle file) {
        auto type = uv_guess_handle(file);

        switch(type) {
        case UV_ASYNC:
            return HandleType::ASYNC;
        case UV_CHECK:
            return HandleType::CHECK;
        case UV_FS_EVENT:
            return HandleType::FS_EVENT;
        case UV_FS_POLL:
            return HandleType::FS_POLL;
        case UV_HANDLE:
            return HandleType::HANDLE;
        case UV_IDLE:
            return HandleType::IDLE;
        case UV_NAMED_PIPE:
            return HandleType::PIPE;
        case UV_POLL:
            return HandleType::POLL;
        case UV_PREPARE:
            return HandleType::PREPARE;
        case UV_PROCESS:
            return HandleType::PROCESS;
        case UV_STREAM:
            return HandleType::STREAM;
        case UV_TCP:
            return HandleType::TCP;
        case UV_TIMER:
            return HandleType::TIMER;
        case UV_TTY:
            return HandleType::TTY;
        case UV_UDP:
            return HandleType::UDP;
        case UV_SIGNAL:
            return HandleType::SIGNAL;
        case UV_FILE:
            return HandleType::FILE;
        default:
            return HandleType::UNKNOWN;
        }
    }

    /** @brief Gets information about the CPUs on the system.
     *
     * This function can be used to query the underlying system and get a set of
     * descriptors of all the available CPUs.
     *
     * @return A set of descriptors of all the available CPUs.
     */
    static std::vector<CPUInfo> cpuInfo() noexcept {
        std::vector<CPUInfo> cpuinfos;

        uv_cpu_info_t *infos;
        int count;

        if(0 == uv_cpu_info(&infos, &count)) {
            std::for_each(infos, infos+count, [&cpuinfos](const auto &info) {
                CPUInfo cpuinfo;

                cpuinfo.model = info.model;
                cpuinfo.speed = info.speed;
                cpuinfo.times = info.cpu_times;

                cpuinfos.push_back(std::move(cpuinfo));
            });

            uv_free_cpu_info(infos, count);
        }

        return cpuinfos;
    }

    /**
     * @brief Gets a set of descriptors of all the available interfaces.
     *
     * This function can be used to query the underlying system and get a set of
     * descriptors of all the available interfaces, either internal or not.
     *
     * @return A set of descriptors of all the available interfaces.
     */
    static std::vector<InterfaceAddress> interfaceAddresses() noexcept {
        std::vector<InterfaceAddress> interfaces;

        uv_interface_address_t *ifaces;
        int count;

        if(0 == uv_interface_addresses(&ifaces, &count)) {
            std::for_each(ifaces, ifaces+count, [&interfaces](const auto &iface) {
                InterfaceAddress interface;

                interface.name = iface.name;
                interface.physical = iface.phys_addr;
                interface.internal = iface.is_internal;

                if(iface.address.address4.sin_family == AF_INET) {
                    interface.address = details::address<IPv4>(&iface.address.address4);
                    interface.netmask = details::address<IPv4>(&iface.netmask.netmask4);
                } else if(iface.address.address4.sin_family == AF_INET6) {
                    interface.address = details::address<IPv6>(&iface.address.address6);
                    interface.netmask = details::address<IPv6>(&iface.netmask.netmask6);
                }

                interfaces.push_back(std::move(interface));
            });

            uv_free_interface_addresses(ifaces, count);
        }

        return interfaces;
    }

    /**
     * @brief Override the use of some standard library’s functions.
     *
     * Override the use of the standard library’s memory allocation
     * functions.

     * This method must be invoked before any other `uvw` function is called or
     * after all resources have been freed and thus the underlying library
     * doesn’t reference any allocated memory chunk.
     *
     * If any of the function pointers is _null_, the invokation will fail.
     *
     * **Note**: there is no protection against changing the allocator multiple
     * times. If the user changes it they are responsible for making sure the
     * allocator is changed while no memory was allocated with the previous
     * allocator, or that they are compatible.
     *
     * @param mallocFunc Replacement function for _malloc_.
     * @param reallocFunc Replacement function for _realloc_.
     * @param callocFunc Replacement function for _calloc_.
     * @param freeFunc Replacement function for _free_.
     * @return True in case of success, false otherwise.
     */
    static bool replaceAllocator(MallocFuncType mallocFunc, ReallocFuncType reallocFunc, CallocFuncType callocFunc, FreeFuncType freeFunc) {
        return (0 == uv_replace_allocator(mallocFunc, reallocFunc, callocFunc, freeFunc));
    }
};

/**
 * TODO
 *
 * * uv_uptime
 * * uv_getrusage
 * * uv_loadavg
 * * uv_exepath
 * * uv_cwd
 * * uv_chdir
 * * uv_os_homedir
 * * uv_os_tmpdir
 * * uv_os_get_passwd
 * * uv_os_free_passwd
 * * uv_get_total_memory
 * * uv_hrtime
 */

}
<|endoftext|>


<commit_before>//=============================================================================================================
/**
* @file     test_spectral_connectivity.cpp
* @author   Daniel Strohmeier <daniel.strohmeier@tu-ilmenau.de>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     May, 2018
*
* @section  LICENSE
*
* Copyright (C) 2018, Daniel Strohmeier and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    The spectral connectivity test implementation
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <utils/ioutils.h>
#include "connectivity/metrics/coherence.h"
#include "connectivity/metrics/imagcoherence.h"
#include "connectivity/metrics/phaselockingvalue.h"
#include "connectivity/metrics/phaselagindex.h"
#include "connectivity/metrics/unbiasedsquaredphaselagindex.h"
#include "connectivity/metrics/weightedphaselagindex.h"
#include "connectivity/metrics/debiasedsquaredweightedphaselagindex.h"

//*************************************************************************************************************
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QtTest>

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Core>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace CONNECTIVITYLIB;
using namespace UTILSLIB;

//=============================================================================================================
/**
* DECLARE CLASS TestSpectralConnectivity
*
* @brief The TestSpectralConnectivity class provides spectral connectivity tests
*
*/
class TestSpectralConnectivity: public QObject
{
    Q_OBJECT

public:
    TestSpectralConnectivity();

private slots:
    void initTestCase();
    void spectralConnectivityCoherence();
    void spectralConnectivityImagCoherence();
    void spectralConnectivityPLV();
    void spectralConnectivityPLI();
    void spectralConnectivityPLI2();
    void spectralConnectivityWPLI();
    void spectralConnectivityWPLI2();
    void cleanupTestCase();

private:
    void compareConnectivity();
    QList<MatrixXd> readConnectivityData();
    double epsilon;
    RowVectorXd m_ConnectivityOutput;
    RowVectorXd m_RefConnectivityOutput;
};

//*************************************************************************************************************

TestSpectralConnectivity::TestSpectralConnectivity()
: epsilon(0.0000001)
{
}

//*************************************************************************************************************

void TestSpectralConnectivity::initTestCase()
{

}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityCoherence()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> Coh = Coherence::computeCoherence(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = Coh.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_coh.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************
    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityImagCoherence()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> ImagCoh = ImagCoherence::computeImagCoherence(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = ImagCoh.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_imagcoh.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLV()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLV = PhaseLockingValue::computePLV(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLV.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_plv.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLI()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLI = PhaseLagIndex::computePLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLI.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_pli.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLI2()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLI2 = UnbiasedSquaredPhaseLagIndex::computeUnbiasedSquaredPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLI2.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_pli2.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityWPLI()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> WPLI = WeightedPhaseLagIndex::computeWPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = WPLI.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_wpli.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityWPLI2()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> WPLI2 = DebiasedSquaredWeightedPhaseLagIndex::computeDebiasedSquaredWPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = WPLI2.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_wpli2.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

QList<MatrixXd> TestSpectralConnectivity::readConnectivityData()
{
    MatrixXd inputTrials;
    QString dataFileName(QDir::currentPath()+"/mne-cpp-test-data/MEG/sample/data_spectral_connectivity.txt");
    IOUtils::read_eigen_matrix(inputTrials, dataFileName);
    int iNTrials = inputTrials.rows() / 2;

    QList<MatrixXd> matDataList;
    for (int i = 0; i < iNTrials; ++i)
    {
        matDataList.append(inputTrials.middleRows(i * 2, 2));
    }

    return matDataList;
}

//*************************************************************************************************************

void TestSpectralConnectivity::compareConnectivity()
{
    //*********************************************************************************************************
    // Compare Spectral Connectvitity Result to MNE-PYTHON
    //*********************************************************************************************************

    printf(">>>>>>>>>>>>>>>>>>>>>>>>> Compare Spectral Connectivities >>>>>>>>>>>>>>>>>>>>>>>>>\n");

    QVERIFY( m_ConnectivityOutput.cols() == m_RefConnectivityOutput.cols() );
    for (int i = 0; i < m_ConnectivityOutput.cols(); ++i)
    {
        if ((m_ConnectivityOutput(i) - m_RefConnectivityOutput(i)) / m_RefConnectivityOutput(i) >= epsilon){
            qDebug() << m_ConnectivityOutput(i);
            qDebug() << m_RefConnectivityOutput(i);
        }

        QVERIFY( abs(m_ConnectivityOutput(i) - m_RefConnectivityOutput(i)) < epsilon );
    }

    printf("<<<<<<<<<<<<<<<<<<<<<<<<< Compare Spectral Connectivities Finished <<<<<<<<<<<<<<<<<<<<<<<<<\n");
}

//*************************************************************************************************************

void TestSpectralConnectivity::cleanupTestCase()
{
}

//*************************************************************************************************************
//=============================================================================================================
// MAIN
//=============================================================================================================

QTEST_APPLESS_MAIN(TestSpectralConnectivity)
#include "test_spectral_connectivity.moc"
<commit_msg>check for zero value in test<commit_after>//=============================================================================================================
/**
* @file     test_spectral_connectivity.cpp
* @author   Daniel Strohmeier <daniel.strohmeier@tu-ilmenau.de>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     May, 2018
*
* @section  LICENSE
*
* Copyright (C) 2018, Daniel Strohmeier and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    The spectral connectivity test implementation
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include <utils/ioutils.h>
#include "connectivity/metrics/coherence.h"
#include "connectivity/metrics/imagcoherence.h"
#include "connectivity/metrics/phaselockingvalue.h"
#include "connectivity/metrics/phaselagindex.h"
#include "connectivity/metrics/unbiasedsquaredphaselagindex.h"
#include "connectivity/metrics/weightedphaselagindex.h"
#include "connectivity/metrics/debiasedsquaredweightedphaselagindex.h"

//*************************************************************************************************************
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QtTest>

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Core>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace CONNECTIVITYLIB;
using namespace UTILSLIB;

//=============================================================================================================
/**
* DECLARE CLASS TestSpectralConnectivity
*
* @brief The TestSpectralConnectivity class provides spectral connectivity tests
*
*/
class TestSpectralConnectivity: public QObject
{
    Q_OBJECT

public:
    TestSpectralConnectivity();

private slots:
    void initTestCase();
    void spectralConnectivityCoherence();
    void spectralConnectivityImagCoherence();
    void spectralConnectivityPLV();
    void spectralConnectivityPLI();
    void spectralConnectivityPLI2();
    void spectralConnectivityWPLI();
    void spectralConnectivityWPLI2();
    void cleanupTestCase();

private:
    void compareConnectivity();
    QList<MatrixXd> readConnectivityData();
    double epsilon;
    RowVectorXd m_ConnectivityOutput;
    RowVectorXd m_RefConnectivityOutput;
};

//*************************************************************************************************************

TestSpectralConnectivity::TestSpectralConnectivity()
: epsilon(0.0000001)
{
}

//*************************************************************************************************************

void TestSpectralConnectivity::initTestCase()
{

}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityCoherence()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> Coh = Coherence::computeCoherence(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = Coh.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_coh.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************
    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityImagCoherence()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> ImagCoh = ImagCoherence::computeImagCoherence(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = ImagCoh.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_imagcoh.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLV()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLV = PhaseLockingValue::computePLV(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLV.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_plv.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLI()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLI = PhaseLagIndex::computePLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLI.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_pli.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityPLI2()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> PLI2 = UnbiasedSquaredPhaseLagIndex::computeUnbiasedSquaredPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = PLI2.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_pli2.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityWPLI()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> WPLI = WeightedPhaseLagIndex::computeWPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = WPLI.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_wpli.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

void TestSpectralConnectivity::spectralConnectivityWPLI2()
{
    //*********************************************************************************************************
    // Load Data
    //*********************************************************************************************************

    QList<MatrixXd> matDataList = readConnectivityData();
    int iNfft = matDataList.at(0).cols();
    QString sWindowType = "hanning";

    //*********************************************************************************************************
    // Compute Connectivity
    //*********************************************************************************************************

    QVector<MatrixXd> WPLI2 = DebiasedSquaredWeightedPhaseLagIndex::computeDebiasedSquaredWPLI(matDataList, iNfft, sWindowType);
    m_ConnectivityOutput = WPLI2.at(0).row(1);

    //*********************************************************************************************************
    // Load MNE-PYTHON Results As Reference
    //*********************************************************************************************************

    MatrixXd refConnectivity;
    QString refFileName(QDir::currentPath()+"/mne-cpp-test-data/Result/Connectivity/ref_spectral_connectivity_wpli2.txt");
    IOUtils::read_eigen_matrix(refConnectivity, refFileName);
    m_RefConnectivityOutput = refConnectivity.col(0).transpose();

    //*********************************************************************************************************
    // Compare Connectivity
    //*********************************************************************************************************

    compareConnectivity();
}

//*************************************************************************************************************

QList<MatrixXd> TestSpectralConnectivity::readConnectivityData()
{
    MatrixXd inputTrials;
    QString dataFileName(QDir::currentPath()+"/mne-cpp-test-data/MEG/sample/data_spectral_connectivity.txt");
    IOUtils::read_eigen_matrix(inputTrials, dataFileName);
    int iNTrials = inputTrials.rows() / 2;

    QList<MatrixXd> matDataList;
    for (int i = 0; i < iNTrials; ++i)
    {
        matDataList.append(inputTrials.middleRows(i * 2, 2));
    }

    return matDataList;
}

//*************************************************************************************************************

void TestSpectralConnectivity::compareConnectivity()
{
    //*********************************************************************************************************
    // Compare Spectral Connectvitity Result to MNE-PYTHON
    //*********************************************************************************************************

    printf(">>>>>>>>>>>>>>>>>>>>>>>>> Compare Spectral Connectivities >>>>>>>>>>>>>>>>>>>>>>>>>\n");

    QVERIFY( m_ConnectivityOutput.cols() == m_RefConnectivityOutput.cols() );
    for (int i = 0; i < m_ConnectivityOutput.cols(); ++i)
    {
        if ((m_ConnectivityOutput(i) - m_RefConnectivityOutput(i)) / m_RefConnectivityOutput(i) >= epsilon){
            qDebug() << m_ConnectivityOutput(i);
            qDebug() << m_RefConnectivityOutput(i);
        }

        if (m_RefConnectivityOutput(i) == 0.0){
            QVERIFY( m_ConnectivityOutput(i) == m_RefConnectivityOutput(i) );
        } else {
            QVERIFY( (abs(m_ConnectivityOutput(i) - m_RefConnectivityOutput(i)) / abs(m_RefConnectivityOutput(i))) < epsilon );
        }
    }

    printf("<<<<<<<<<<<<<<<<<<<<<<<<< Compare Spectral Connectivities Finished <<<<<<<<<<<<<<<<<<<<<<<<<\n");
}

//*************************************************************************************************************

void TestSpectralConnectivity::cleanupTestCase()
{
}

//*************************************************************************************************************
//=============================================================================================================
// MAIN
//=============================================================================================================

QTEST_APPLESS_MAIN(TestSpectralConnectivity)
#include "test_spectral_connectivity.moc"
<|endoftext|>


<commit_before>#include<stdio.h>
#include<iostream>
#include<ros/ros.h>
#include<steer_drive_controller/steer_drive_controller.h>

// ros_control
#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
// gazebo
#include <control_toolbox/pid.h>
#include <gazebo_ros_control/robot_hw_sim.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

int configureVelocityJointInterface(hardware_interface::VelocityJointInterface&);

class SteerBot:
        //public gazebo_ros_control::RobotHWSim
        public hardware_interface::RobotHW
{
    // methods
public:
    SteerBot(ros::NodeHandle &_nh)
        : ns_("steer_drive_controller/")
    {
        this->CleanUp();
#ifdef MULTIPLE_JOINTS
        this->GetJointNames(_nh);
        this->Resize();
#endif
        this->RegisterHardwareInterfaces();
    }

    bool initSim(const std::string& _robot_namespace,
      ros::NodeHandle _nh,
      gazebo::physics::ModelPtr _model,
      const urdf::Model* const _urdf_model,
      std::vector<transmission_interface::TransmissionInfo> _transmissions)
    {
        using gazebo::physics::JointPtr;

        this->CleanUp();

        // simulation joints
        sim_joints_ = _model->GetJoints();
        int n_dof = sim_joints_.size();

#ifdef MULTIPLE_JOINTS
        this->GetJointNames(_nh);
        this->Resize();
#endif
        this->RegisterHardwareInterfaces();
    }

    void readSim(ros::Time time, ros::Duration period)
    {

    }

    void writeSim(ros::Time time, ros::Duration period)
    {

    }

    void read()
    {
        std::ostringstream os;
        for (unsigned int i = 0; i < cnt_wheel_joints_ - 1; ++i)
        {
            os << wheel_joint_vel_cmd_[i] << ", ";
        }
        os << wheel_joint_vel_cmd_[cnt_wheel_joints_ - 1];

        ROS_INFO_STREAM("Commands for wheel joints: " << os.str());
    }

    ros::Duration getPeriod() const {return ros::Duration(0.01);}

    void write()
    {
        bool running_ = true;
        if (running_)
        {
            for (unsigned int i = 0; i < cnt_wheel_joints_; ++i)
            {
                // Note that pos_[i] will be NaN for one more cycle after we start(),
                // but that is consistent with the knowledge we have about the state
                // of the robot.
                wheel_joint_pos_[i] += wheel_joint_vel_[i]*getPeriod().toSec(); // update position
                wheel_joint_vel_[i] = wheel_joint_vel_cmd_[i]; // might add smoothing here later
            }
        }
#if 0
        else
        {
            std::fill_n(joint_pos_, cnt_wheel_joints_, std::numeric_limits<double>::quiet_NaN());
            std::fill_n(joint_vel_, cnt_wheel_joints_, std::numeric_limits<double>::quiet_NaN());
        }
#endif
    }
private:
    void CleanUp()
    {
#ifdef MULTIPLE_JOINTS
       // wheel joints
       wheel_joint_names_.clear();
       wheel_joint_pos_.clear();
       wheel_joint_vel_.clear();
       wheel_joint_eff_.clear();
       wheel_joint_vel_cmd_.clear();
#endif
       // rear wheel joint
        wheel_jnt_pos_ = 0;
        wheel_jnt_vel_ = 0;
        wheel_jnt_eff_ = 0;
        wheel_jnt_vel_cmd_ = 0;

#ifdef MULTIPLE_JOINTS
       // steer joints
       steer_joint_names_.clear();
       steer_joint_pos_.clear();
       steer_joint_vel_.clear();
       steer_joint_eff_.clear();
       steer_joint_vel_cmd_.clear();
#endif
       // front steer joint
       steer_jnt_pos_ = 0;
       steer_jnt_vel_ = 0;
       steer_jnt_eff_ = 0;
       steer_jnt_pos_cmd_ = 0;

    }

#ifdef MULTIPLE_JOINTS
    void Resize()
    {
       // wheel joints
       wheel_joint_pos_.resize(cnt_wheel_joints_);
       wheel_joint_vel_.resize(cnt_wheel_joints_);
       wheel_joint_eff_.resize(cnt_wheel_joints_);
       wheel_joint_vel_cmd_.resize(cnt_wheel_joints_);

       // steer joints
       steer_joint_pos_.resize(cnt_steer_joints_);
       steer_joint_vel_.resize(cnt_steer_joints_);
       steer_joint_eff_.resize(cnt_steer_joints_);
       steer_joint_vel_cmd_.resize(cnt_steer_joints_);
    }

    void GetJointNames(ros::NodeHandle &_nh)
    {
        this->GetWheelJointNames(_nh);
        this->GetSteerJointNames(_nh);

        _nh.getParam(ns_ + "rear_wheel", wheel_jnt_name_);
        _nh.getParam(ns_ + "front_steer", steer_jnt_name_);

    }

    void GetWheelJointNames(ros::NodeHandle &_nh)
    {
        // wheel names
        std::vector<std::string> right_wheel_joint_names;
        std::vector<std::string> left_wheel_joint_names;

        _nh.getParam(ns_ + "right_wheel", right_wheel_joint_names);
        _nh.getParam(ns_ + "left_wheel", left_wheel_joint_names);

        wheel_joint_names_ = right_wheel_joint_names;
        std::copy(left_wheel_joint_names.begin(), left_wheel_joint_names.end(),
                  std::back_inserter(wheel_joint_names_));
        cnt_wheel_joints_ = wheel_joint_names_.size();

        // rear wheel name
        std::string wheel_joint_name;
        /*
        _nh.getParam(ns_ + "rear_wheel", wheel_joint_name);
        wheel_joint_name_ = wheel_joint_name;
        */
    }

    void GetSteerJointNames(ros::NodeHandle &_nh)
    {
        // steer names
        std::vector<std::string> right_steer_joint_names;
        std::vector<std::string> left_steer_joint_names;

        _nh.getParam(ns_ + "right_steer", right_steer_joint_names);
        _nh.getParam(ns_ + "left_steer", left_steer_joint_names);

        steer_joint_names_ = right_steer_joint_names;
        std::copy(left_steer_joint_names.begin(), left_steer_joint_names.end(),
                  std::back_inserter(steer_joint_names_));
        cnt_steer_joints_ = steer_joint_names_.size();

        // front steer name
        std::string steer_joint_name;
        /*
        _nh.getParam(ns_ + "front_steer", steer_joint_name);
        wheel_joint_name_ = steer_joint_name;
        */
    }
#endif

    void RegisterHardwareInterfaces()
    {
        this->RegisterSteerInterfaces();
        hardware_interface::JointStateHandle state_handle_steer(wheel_jnt_name_,
                                                                &wheel_jnt_pos_,
                                                                &wheel_jnt_vel_,
                                                                &wheel_jnt_eff_);
        wheel_joint_state_interface_.registerHandle(state_handle_steer);
        // joint velocity command
        hardware_interface::JointHandle vel_handle(wheel_joint_state_interface_.getHandle(wheel_jnt_name_),
                                                   &wheel_jnt_vel_cmd_);
        wheel_vel_joint_interface_.registerHandle(vel_handle);

        ROS_DEBUG_STREAM("Registered joint '" << wheel_jnt_name_ << " ' in the VelocityJointInterface");

        this->RegisterWheelInterfaces();
        hardware_interface::JointStateHandle state_handle_wheel(steer_jnt_name_,
                                                                &steer_jnt_pos_,
                                                                &steer_jnt_vel_,
                                                                &steer_jnt_eff_);
        steer_joint_state_interface_.registerHandle(state_handle_wheel);

        // joint position command
        hardware_interface::JointHandle pos_handle(steer_joint_state_interface_.getHandle(steer_jnt_name_),
                                                   &steer_jnt_pos_cmd_);
        steer_pos_joint_interface_.registerHandle(pos_handle);

        ROS_DEBUG_STREAM("Registered joint '" << steer_jnt_name_ << " ' in the PositionJointInterface");
    }

    void RegisterWheelInterfaces()
    {
        // map members to hardware interfaces
        for (size_t i = 0; i < cnt_wheel_joints_; ++i) {
            // joint states
            hardware_interface::JointStateHandle state_handle(wheel_joint_names_[i],
                                                              &wheel_joint_pos_[i],
                                                              &wheel_joint_vel_[i],
                                                              &wheel_joint_eff_[i]);
            wheel_joint_state_interface_.registerHandle(state_handle);

            // joint velocity command
            hardware_interface::JointHandle vel_handle(wheel_joint_state_interface_.getHandle(wheel_joint_names_[i]),
                                                       &wheel_joint_vel_cmd_[i]);
            wheel_vel_joint_interface_.registerHandle(vel_handle);

            ROS_DEBUG_STREAM("Registered joint '" << wheel_joint_names_[i] << " ' in the VelocityJointInterface");
        }

        // register mapped interface to ros_control
        registerInterface(&wheel_joint_state_interface_);
        registerInterface(&wheel_vel_joint_interface_);
    }

    void RegisterSteerInterfaces()
    {
        // map members to hardware interfaces
        for (size_t i = 0; i < cnt_steer_joints_; ++i) {
            // joint states
            hardware_interface::JointStateHandle state_handle(steer_joint_names_[i],
                                                              &steer_joint_pos_[i],
                                                              &steer_joint_vel_[i],
                                                              &steer_joint_eff_[i]);
            steer_joint_state_interface_.registerHandle(state_handle);

            // joint position command
            hardware_interface::JointHandle pos_handle(steer_joint_state_interface_.getHandle(steer_joint_names_[i]),
                                                       &steer_joint_vel_cmd_[i]);
            steer_pos_joint_interface_.registerHandle(pos_handle);

            ROS_DEBUG_STREAM("Registered joint '" << steer_joint_names_[i] << " ' in the PositionJointInterface");
        }

        // register mapped interface to ros_control
        registerInterface(&steer_joint_state_interface_);
        registerInterface(&steer_pos_joint_interface_);
    }

    // member variables
public:
    hardware_interface::VelocityJointInterface wheel_vel_joint_interface_;
    hardware_interface::PositionJointInterface steer_pos_joint_interface_;

private:
    ros::NodeHandle nh_;

    std::string ns_;
    //
    std::vector<std::string> wheel_joint_names_;
    std::vector<std::string> steer_joint_names_;
    std::string wheel_jnt_name_;
    std::string steer_jnt_name_;

    // interface variables
    //-- wheel
    int cnt_wheel_joints_;
    std::vector<double> wheel_joint_pos_;
    std::vector<double> wheel_joint_vel_;
    std::vector<double> wheel_joint_eff_;
    std::vector<double> wheel_joint_vel_cmd_;
    //-- rear wheel
    double wheel_jnt_pos_;
    double wheel_jnt_vel_;
    double wheel_jnt_eff_;
    double wheel_jnt_vel_cmd_;

    //-- steer
    int cnt_steer_joints_;
    std::vector<double> steer_joint_pos_;
    std::vector<double> steer_joint_vel_;
    std::vector<double> steer_joint_eff_;
    std::vector<double> steer_joint_vel_cmd_;
    //-- front steer
    double steer_jnt_pos_;
    double steer_jnt_vel_;
    double steer_jnt_eff_;
    double steer_jnt_pos_cmd_;

    //
    hardware_interface::JointStateInterface wheel_joint_state_interface_;
    hardware_interface::JointStateInterface steer_joint_state_interface_;

    // gazebo
    std::vector<gazebo::physics::JointPtr> sim_joints_;

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "steer_drive_test");
    ros::NodeHandle nh_main;
    ros::NodeHandle nh_control;

    SteerBot* steer_drive_bot = new SteerBot(nh_control);

    steer_drive_controller::SteerDriveController rb_controller;
#if 0
    bool rt_code = rb_controller.init(//&steer_drive_bot->wheel_vel_joint_interface_,
                                      steer_drive_bot,
                                      nh_main, nh_control);
#endif
    std::set<std::string> claimed_resources; // Gets populated during initRequest call
    bool rt_code = rb_controller.init(steer_drive_bot, nh_main, nh_control);//, claimed_resources);

    ros::Rate rate(100);
    ros::Time last_time = ros::Time::now();

    while(nh_main.ok())
    {
        ros::Time now = ros::Time::now();
        ros::Duration period = now - last_time;
        last_time = now;

        steer_drive_bot->read();
        rb_controller.update(now, period);
        steer_drive_bot->write();

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
<commit_msg>[WIP] 単一のwheel, steer のjointに指令値を遅れるようにした．<commit_after>#include<stdio.h>
#include<iostream>
#include<ros/ros.h>
#include<steer_drive_controller/steer_drive_controller.h>

// ros_control
#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
// gazebo
#include <control_toolbox/pid.h>
#include <gazebo_ros_control/robot_hw_sim.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>

int configureVelocityJointInterface(hardware_interface::VelocityJointInterface&);

class SteerBot:
        //public gazebo_ros_control::RobotHWSim
        public hardware_interface::RobotHW
{
    // methods
public:
    SteerBot(ros::NodeHandle &_nh)
        : ns_("steer_drive_controller/")
    {
        this->CleanUp();
        this->GetJointNames(_nh);
#ifdef MULTIPLE_JOINTS
        this->Resize();
#endif
        this->RegisterHardwareInterfaces();
    }

    bool initSim(const std::string& _robot_namespace,
      ros::NodeHandle _nh,
      gazebo::physics::ModelPtr _model,
      const urdf::Model* const _urdf_model,
      std::vector<transmission_interface::TransmissionInfo> _transmissions)
    {
        using gazebo::physics::JointPtr;

        this->CleanUp();

        // simulation joints
        sim_joints_ = _model->GetJoints();
        int n_dof = sim_joints_.size();

#ifdef MULTIPLE_JOINTS
        this->GetJointNames(_nh);
        this->Resize();
#endif
        this->RegisterHardwareInterfaces();
    }

    void readSim(ros::Time time, ros::Duration period)
    {

    }

    void writeSim(ros::Time time, ros::Duration period)
    {

    }

    void read()
    {
        std::ostringstream os;

#ifdef MULTIPLE_JOINTS
        for (unsigned int i = 0; i < cnt_wheel_joints_ - 1; ++i)
        {
            os << wheel_joint_vel_cmd_[i] << ", ";
        }
        os << wheel_joint_vel_cmd_[cnt_wheel_joints_ - 1];
#endif
        os << wheel_jnt_vel_cmd_;

        ROS_INFO_STREAM("Commands for wheel joints: " << os.str());
    }

    ros::Duration getPeriod() const {return ros::Duration(0.01);}

    void write()
    {
        bool running_ = true;
        if (running_)
        {
#ifdef MULTIPLE_JOINTS
            for (unsigned int i = 0; i < cnt_wheel_joints_; ++i)
            {
                // Note that pos_[i] will be NaN for one more cycle after we start(),
                // but that is consistent with the knowledge we have about the state
                // of the robot.
                wheel_joint_pos_[i] += wheel_joint_vel_[i]*getPeriod().toSec(); // update position
                wheel_joint_vel_[i] = wheel_joint_vel_cmd_[i]; // might add smoothing here later
            }
#endif
            wheel_jnt_pos_ += wheel_jnt_vel_ * getPeriod().toSec(); // update position
            wheel_jnt_vel_ = wheel_jnt_vel_cmd_;
        }
#if 0
        else
        {
            std::fill_n(joint_pos_, cnt_wheel_joints_, std::numeric_limits<double>::quiet_NaN());
            std::fill_n(joint_vel_, cnt_wheel_joints_, std::numeric_limits<double>::quiet_NaN());
        }
#endif
    }
private:
    void CleanUp()
    {
#ifdef MULTIPLE_JOINTS
       // wheel joints
       wheel_joint_names_.clear();
       wheel_joint_pos_.clear();
       wheel_joint_vel_.clear();
       wheel_joint_eff_.clear();
       wheel_joint_vel_cmd_.clear();
#endif
       // rear wheel joint
        wheel_jnt_pos_ = 0;
        wheel_jnt_vel_ = 0;
        wheel_jnt_eff_ = 0;
        wheel_jnt_vel_cmd_ = 0;

#ifdef MULTIPLE_JOINTS
       // steer joints
       steer_joint_names_.clear();
       steer_joint_pos_.clear();
       steer_joint_vel_.clear();
       steer_joint_eff_.clear();
       steer_joint_vel_cmd_.clear();
#endif
       // front steer joint
       steer_jnt_pos_ = 0;
       steer_jnt_vel_ = 0;
       steer_jnt_eff_ = 0;
       steer_jnt_pos_cmd_ = 0;

    }

#ifdef MULTIPLE_JOINTS
    void Resize()
    {
       // wheel joints
       wheel_joint_pos_.resize(cnt_wheel_joints_);
       wheel_joint_vel_.resize(cnt_wheel_joints_);
       wheel_joint_eff_.resize(cnt_wheel_joints_);
       wheel_joint_vel_cmd_.resize(cnt_wheel_joints_);

       // steer joints
       steer_joint_pos_.resize(cnt_steer_joints_);
       steer_joint_vel_.resize(cnt_steer_joints_);
       steer_joint_eff_.resize(cnt_steer_joints_);
       steer_joint_vel_cmd_.resize(cnt_steer_joints_);
    }
#endif
    void GetJointNames(ros::NodeHandle &_nh)
    {
#ifdef MULTIPLE_JOINTS
        this->GetWheelJointNames(_nh);
        this->GetSteerJointNames(_nh);
#endif

        _nh.getParam(ns_ + "rear_wheel", wheel_jnt_name_);
        _nh.getParam(ns_ + "front_steer", steer_jnt_name_);

    }

#ifdef MULTIPLE_JOINTS
    void GetWheelJointNames(ros::NodeHandle &_nh)
    {
        // wheel names
        std::vector<std::string> right_wheel_joint_names;
        std::vector<std::string> left_wheel_joint_names;

        _nh.getParam(ns_ + "right_wheel", right_wheel_joint_names);
        _nh.getParam(ns_ + "left_wheel", left_wheel_joint_names);

        wheel_joint_names_ = right_wheel_joint_names;
        std::copy(left_wheel_joint_names.begin(), left_wheel_joint_names.end(),
                  std::back_inserter(wheel_joint_names_));
        cnt_wheel_joints_ = wheel_joint_names_.size();

        // rear wheel name
        std::string wheel_joint_name;
        /*
        _nh.getParam(ns_ + "rear_wheel", wheel_joint_name);
        wheel_joint_name_ = wheel_joint_name;
        */
    }

    void GetSteerJointNames(ros::NodeHandle &_nh)
    {
        // steer names
        std::vector<std::string> right_steer_joint_names;
        std::vector<std::string> left_steer_joint_names;

        _nh.getParam(ns_ + "right_steer", right_steer_joint_names);
        _nh.getParam(ns_ + "left_steer", left_steer_joint_names);

        steer_joint_names_ = right_steer_joint_names;
        std::copy(left_steer_joint_names.begin(), left_steer_joint_names.end(),
                  std::back_inserter(steer_joint_names_));
        cnt_steer_joints_ = steer_joint_names_.size();

        // front steer name
        std::string steer_joint_name;
        /*
        _nh.getParam(ns_ + "front_steer", steer_joint_name);
        wheel_joint_name_ = steer_joint_name;
        */
    }
#endif

    void RegisterHardwareInterfaces()
    {
#ifdef MULTIPLE_JOINTS
        this->RegisterSteerInterfaces();
#endif
        this->RegisterSteerInterface();

#ifdef MULTIPLE_JOINTS
        this->RegisterWheelInterfaces();
#endif
        this->RegisterWheelInterface();
    }

    void RegisterWheelInterface()
    {
        hardware_interface::JointStateHandle state_handle_steer(wheel_jnt_name_,
                                                                &wheel_jnt_pos_,
                                                                &wheel_jnt_vel_,
                                                                &wheel_jnt_eff_);
        wheel_joint_state_interface_.registerHandle(state_handle_steer);
        // joint velocity command
        hardware_interface::JointHandle vel_handle(wheel_joint_state_interface_.getHandle(wheel_jnt_name_),
                                                   &wheel_jnt_vel_cmd_);
        wheel_vel_joint_interface_.registerHandle(vel_handle);

        ROS_DEBUG_STREAM("Registered joint '" << wheel_jnt_name_ << " ' in the VelocityJointInterface");

        // register mapped interface to ros_control
        registerInterface(&wheel_joint_state_interface_);
        registerInterface(&wheel_vel_joint_interface_);
    }

    void RegisterWheelInterfaces()
    {
        // map members to hardware interfaces
        for (size_t i = 0; i < cnt_wheel_joints_; ++i) {
            // joint states
            hardware_interface::JointStateHandle state_handle(wheel_joint_names_[i],
                                                              &wheel_joint_pos_[i],
                                                              &wheel_joint_vel_[i],
                                                              &wheel_joint_eff_[i]);
            wheel_joint_state_interface_.registerHandle(state_handle);

            // joint velocity command
            hardware_interface::JointHandle vel_handle(wheel_joint_state_interface_.getHandle(wheel_joint_names_[i]),
                                                       &wheel_joint_vel_cmd_[i]);
            wheel_vel_joint_interface_.registerHandle(vel_handle);

            ROS_DEBUG_STREAM("Registered joint '" << wheel_joint_names_[i] << " ' in the VelocityJointInterface");
        }

        // register mapped interface to ros_control
        registerInterface(&wheel_joint_state_interface_);
        registerInterface(&wheel_vel_joint_interface_);
    }

    void RegisterSteerInterface()
    {
        hardware_interface::JointStateHandle state_handle_wheel(steer_jnt_name_,
                                                                &steer_jnt_pos_,
                                                                &steer_jnt_vel_,
                                                                &steer_jnt_eff_);
        steer_joint_state_interface_.registerHandle(state_handle_wheel);

        // joint position command
        hardware_interface::JointHandle pos_handle(steer_joint_state_interface_.getHandle(steer_jnt_name_),
                                                   &steer_jnt_pos_cmd_);
        steer_pos_joint_interface_.registerHandle(pos_handle);

        ROS_DEBUG_STREAM("Registered joint '" << steer_jnt_name_ << " ' in the PositionJointInterface");

        // register mapped interface to ros_control
        registerInterface(&steer_joint_state_interface_);
        registerInterface(&steer_pos_joint_interface_);
    }

    void RegisterSteerInterfaces()
    {
        // map members to hardware interfaces
        for (size_t i = 0; i < cnt_steer_joints_; ++i) {
            // joint states
            hardware_interface::JointStateHandle state_handle(steer_joint_names_[i],
                                                              &steer_joint_pos_[i],
                                                              &steer_joint_vel_[i],
                                                              &steer_joint_eff_[i]);
            steer_joint_state_interface_.registerHandle(state_handle);

            // joint position command
            hardware_interface::JointHandle pos_handle(steer_joint_state_interface_.getHandle(steer_joint_names_[i]),
                                                       &steer_joint_vel_cmd_[i]);
            steer_pos_joint_interface_.registerHandle(pos_handle);

            ROS_DEBUG_STREAM("Registered joint '" << steer_joint_names_[i] << " ' in the PositionJointInterface");
        }

        // register mapped interface to ros_control
        registerInterface(&steer_joint_state_interface_);
        registerInterface(&steer_pos_joint_interface_);
    }

    // member variables
public:
    hardware_interface::VelocityJointInterface wheel_vel_joint_interface_;
    hardware_interface::PositionJointInterface steer_pos_joint_interface_;

private:
    ros::NodeHandle nh_;

    std::string ns_;
    //
    std::vector<std::string> wheel_joint_names_;
    std::vector<std::string> steer_joint_names_;
    std::string wheel_jnt_name_;
    std::string steer_jnt_name_;

    // interface variables
    //-- wheel
    int cnt_wheel_joints_;
    std::vector<double> wheel_joint_pos_;
    std::vector<double> wheel_joint_vel_;
    std::vector<double> wheel_joint_eff_;
    std::vector<double> wheel_joint_vel_cmd_;
    //-- rear wheel
    double wheel_jnt_pos_;
    double wheel_jnt_vel_;
    double wheel_jnt_eff_;
    double wheel_jnt_vel_cmd_;

    //-- steer
    int cnt_steer_joints_;
    std::vector<double> steer_joint_pos_;
    std::vector<double> steer_joint_vel_;
    std::vector<double> steer_joint_eff_;
    std::vector<double> steer_joint_vel_cmd_;
    //-- front steer
    double steer_jnt_pos_;
    double steer_jnt_vel_;
    double steer_jnt_eff_;
    double steer_jnt_pos_cmd_;

    //
    hardware_interface::JointStateInterface wheel_joint_state_interface_;
    hardware_interface::JointStateInterface steer_joint_state_interface_;

    // gazebo
    std::vector<gazebo::physics::JointPtr> sim_joints_;

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "steer_drive_test");
    ros::NodeHandle nh_main;
    ros::NodeHandle nh_control;

    SteerBot* steer_drive_bot = new SteerBot(nh_control);

    steer_drive_controller::SteerDriveController rb_controller;
#if 0
    bool rt_code = rb_controller.init(//&steer_drive_bot->wheel_vel_joint_interface_,
                                      steer_drive_bot,
                                      nh_main, nh_control);
#endif
    std::set<std::string> claimed_resources; // Gets populated during initRequest call
    bool rt_code = rb_controller.init(steer_drive_bot, nh_main, nh_control);//, claimed_resources);

    ros::Rate rate(100);
    ros::Time last_time = ros::Time::now();

    while(nh_main.ok())
    {
        ros::Time now = ros::Time::now();
        ros::Duration period = now - last_time;
        last_time = now;

        steer_drive_bot->read();
        rb_controller.update(now, period);
        steer_drive_bot->write();

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
<|endoftext|>