enpose_api.hpp

 
#pragma once
 
#include "enpose_api.h"
 
#include <cstddef>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
 
namespace enpose {
 
using MarkerPose = ::EnposeMarkerPose;
 
class Error : public std::runtime_error {
public:
    explicit Error(const std::string& message) : std::runtime_error(message) {}
};
 
struct DeviceInfo {
    std::string ip;        
    std::uint32_t serial;  
    bool compatible;       
};
 
inline std::vector<DeviceInfo> discover() {
    EnposeDeviceInfo* devices = nullptr;
    std::size_t count = 0;
    if (enpose_discover(&devices, &count) != ENPOSE_OK) {
        throw Error("enpose_discover failed");
    }
    std::vector<DeviceInfo> result;
    result.reserve(count);
    for (std::size_t i = 0; i < count; ++i) {
        result.push_back(DeviceInfo{std::string(devices[i].ip), devices[i].serial,
                                    devices[i].compatible});
    }
    enpose_device_info_array_free(devices, count);
    return result;
}
 
class PoseStream {
public:
    explicit PoseStream(const std::string& ip, bool create_thread = true)
        : handle_(enpose_pose_stream_connect(ip.c_str(), create_thread)) {
        if (handle_ == nullptr) {
            throw Error("failed to connect pose stream to " + ip);
        }
    }
 
    explicit PoseStream(const DeviceInfo& device, bool create_thread = true)
        : PoseStream(device.ip, create_thread) {}
 
    ~PoseStream() {
        if (handle_ != nullptr) {
            enpose_pose_stream_free(handle_);
        }
    }
 
    PoseStream(const PoseStream&) = delete;
    PoseStream& operator=(const PoseStream&) = delete;
 
    PoseStream(PoseStream&& other) noexcept
        : handle_(std::exchange(other.handle_, nullptr)) {}
 
    PoseStream& operator=(PoseStream&& other) noexcept {
        if (this != &other) {
            if (handle_ != nullptr) {
                enpose_pose_stream_free(handle_);
            }
            handle_ = std::exchange(other.handle_, nullptr);
        }
        return *this;
    }
 
    std::vector<MarkerPose> receive(bool block = false) {
        EnposeMarkerPose* poses = nullptr;
        std::size_t count = 0;
        if (enpose_pose_stream_receive(handle_, block, &poses, &count) != ENPOSE_OK) {
            throw Error("enpose_pose_stream_receive failed");
        }
        std::vector<MarkerPose> result(poses, poses + count);
        enpose_marker_pose_array_free(poses, count);
        return result;
    }
 
private:
    EnposePoseStream* handle_;
};
 
}  // namespace enpose