InterpolatedPositionBuffer.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __INTERPOLATED_POSITION_BUFFER_HPP__
#define __INTERPOLATED_POSITION_BUFFER_HPP__
 
#include <cstdint>
 
#include <deque>
#include <mutex>
#include <vector>
 
namespace roboticslab
{
 
class InterpolatedPositionBuffer
{
public:
    InterpolatedPositionBuffer(double samplingPeriod, double interpolationPeriod);
 
    virtual ~InterpolatedPositionBuffer() = default;
 
    void setInitial(int initialTarget);
 
    int getPeriodMs() const;
 
    virtual std::uint16_t getBufferSize() const = 0;
 
    std::uint16_t getBufferConfig() const;
 
    virtual std::int16_t getSubMode() const = 0;
 
    void addSetpoint(int target);
 
    std::vector<std::uint64_t> popBatch(bool fullBuffer);
 
    int getPrevTarget() const;
 
    bool isQueueReady() const;
 
    bool isQueueEmpty() const;
 
    void clearQueue();
 
protected:
    using ip_record = std::pair<int, double>; // position (internal units), timestamp (seconds)
 
    std::uint8_t getIntegrityCounter() const;
 
    virtual std::size_t getOffset() const;
 
    std::uint16_t getSampledTime(double currentTimestamp);
 
    double getMeanVelocity(const ip_record & earliest, const ip_record & latest) const;
 
    virtual std::uint64_t makeDataRecord(const ip_record & previous, const ip_record & current, const ip_record & next) = 0;
 
    const double samplingPeriod; // [s]
 
private:
    const std::uint16_t fixedSamples;
    std::uint8_t integrityCounter;
    ip_record prevTarget;
    double initialTimestamp;
    int sampleCount;
    std::deque<ip_record> pendingTargets;
    mutable std::mutex queueMutex;
};
 
class PtBuffer : public InterpolatedPositionBuffer
{
public:
    using InterpolatedPositionBuffer::InterpolatedPositionBuffer;
 
    std::uint16_t getBufferSize() const override;
    std::int16_t getSubMode() const override;
 
protected:
    std::uint64_t makeDataRecord(const ip_record & previous, const ip_record & current, const ip_record & next) override;
};
 
class PvtBuffer : public InterpolatedPositionBuffer
{
public:
    using InterpolatedPositionBuffer::InterpolatedPositionBuffer;
 
    std::uint16_t getBufferSize() const override;
    std::int16_t getSubMode() const override;
 
protected:
    std::size_t getOffset() const override;
    std::uint64_t makeDataRecord(const ip_record & previous, const ip_record & current, const ip_record & next) override;
};
 
} // namespace roboticslab
 
#endif // __INTERPOLATED_POSITION_BUFFER_HPP__