DeviceMapper.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __DEVICE_MAPPER_HPP__
#define __DEVICE_MAPPER_HPP__
 
#include <cstdlib> // std::size_t
 
#include <functional> // std::invoke
#include <memory>
#include <tuple>
#include <typeindex>
#include <unordered_map>
#include <vector>
 
#include <yarp/dev/PolyDriver.h>
 
#include "FutureTask.hpp"
#include "RawDevice.hpp"
 
namespace roboticslab
{
 
class DeviceMapper final
{
public:
    DeviceMapper();
 
    ~DeviceMapper();
 
    void enableParallelization(unsigned int concurrentTasks);
 
    bool registerDevice(yarp::dev::PolyDriver * driver);
 
    void clear();
 
    std::unique_ptr<FutureTask> createTask() const
    { return taskFactory->createTask(); }
 
    using dev_index_t = std::tuple<const RawDevice *, int>;
 
    using dev_group_t = std::tuple<const RawDevice *, std::vector<int>, int>;
 
    const std::vector<std::unique_ptr<const RawDevice>> & getDevices() const
    { return devices; }
 
    dev_index_t getMotorDevice(int globalAxis) const;
 
    std::vector<dev_index_t> getMotorDevicesWithOffsets() const;
 
    std::vector<dev_group_t> getMotorDevicesWithIndices(int globalAxesCount, const int * globalAxes) const;
 
    int getControlledAxes() const
    { return totalAxes; }
 
    template<typename T, typename... T_ref>
    using motor_single_joint_fn = bool (T::*)(int, T_ref...);
 
    template<typename T, typename... T_ref>
    bool mapSingleJoint(motor_single_joint_fn<T, T_ref...> fn, int j, T_ref... ref)
    {
        auto [device, offset] = getMotorDevice(j);
        T * p = device->getHandle<T>();
        return p ? std::invoke(fn, p, offset, ref...) : false;
    }
 
    template<typename T, typename... T_refs>
    using motor_all_joints_fn = bool (T::*)(T_refs *...);
 
    template<typename T, typename... T_refs>
    bool mapAllJoints(motor_all_joints_fn<T, T_refs...> fn, T_refs *... refs)
    {
        auto task = createTask();
        bool ok = false;
 
        for (const auto & [device, offset] : getMotorDevicesWithOffsets())
        {
            T * p = device->template getHandle<T>();
            ok |= p && (task->add(p, fn, refs + offset...), true);
        }
 
        // at least one targeted device must implement the 'T' iface
        return ok && task->dispatch();
    }
 
    template<typename T, typename... T_refs>
    using motor_multi_joints_fn = bool (T::*)(int, const int *, T_refs *...);
 
    template<typename T, typename... T_refs>
    bool mapJointGroup(motor_multi_joints_fn<T, T_refs...> fn, int n_joint, const int * joints, T_refs *... refs)
    {
        auto task = createTask();
        auto devices = getMotorDevicesWithIndices(n_joint, joints); // extend lifetime of vector of local indices
        bool ok = true;
 
        for (const auto & [device, localIndices, globalIndex] : devices)
        {
            T * p = device->template getHandle<T>();
            ok &= p && (task->add(p, fn, localIndices.size(), localIndices.data(), refs + globalIndex...), true);
        }
 
        // all targeted devices must implement the 'T' iface
        return ok && task->dispatch();
    }
 
    template<typename T>
    int getConnectedSensors() const
    {
        // operator[] will insert a default-constructed value if not found
        if (auto it = connectedSensors.find(typeid(T)); it != connectedSensors.cend())
        {
            return it->second;
        }
 
        return 0;
    }
 
    template<typename T>
    dev_index_t getSensorDevice(int globalIndex) const
    {
        if (auto it = sensorOffsets.find(typeid(T)); it != sensorOffsets.cend())
        {
            const auto & [deviceIndex, offset, count] = it->second[globalIndex];
            return {devices[deviceIndex].get(), globalIndex - offset};
        }
 
        return {&invalidDevice, 0};
    }
 
    template<typename T, typename T_out>
    using sensor_status_fn = T_out (T::*)(std::size_t) const;
 
    template<typename T, typename T_out>
    T_out getSensorStatus(sensor_status_fn<T, T_out> fn, std::size_t index) const
    {
        auto [device, offset] = getSensorDevice<T>(index);
        T * p = device->template getHandle<T>();
        return p ? std::invoke(fn, p, offset) : static_cast<T_out>(DeviceMapper::getSensorFailureStatus());
    }
 
    template<typename T>
    using sensor_size_fn = std::size_t (T::*)(std::size_t) const;
 
    template<typename T>
    std::size_t getSensorArraySize(sensor_size_fn<T> fn, std::size_t index) const
    {
        auto [device, offset] = getSensorDevice<T>(index);
        T * p = device->template getHandle<T>();
        return p ? std::invoke(fn, p, offset) : 0;
    }
 
    template<typename T, typename... T_out_params>
    using sensor_output_fn = bool (T::*)(std::size_t, T_out_params &...) const;
 
    template<typename T, typename... T_out_params>
    bool getSensorOutput(sensor_output_fn<T, T_out_params...> fn, std::size_t index, T_out_params &... params) const
    {
        auto [device, offset] = getSensorDevice<T>(index);
        T * p = device->template getHandle<T>();
        return p ? std::invoke(fn, p, offset, params...) : false;
    }
 
private:
    static const int getSensorFailureStatus();
 
    using dev_index_offset_t = std::tuple<int, int, int>;
 
    std::vector<std::unique_ptr<const RawDevice>> devices;
    std::vector<dev_index_offset_t> motorOffsets;
    std::unordered_map<std::type_index, std::vector<dev_index_offset_t>> sensorOffsets;
    std::unordered_map<std::type_index, int> connectedSensors;
 
    int totalAxes {0};
 
    std::unique_ptr<FutureTaskFactory> taskFactory;
};
 
} // namespace roboticslab
 
#endif // __DEVICE_MAPPER_HPP__