001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.interpolation; 006 007import edu.wpi.first.math.MathUtil; 008import java.util.NavigableMap; 009import java.util.Optional; 010import java.util.TreeMap; 011 012/** 013 * The TimeInterpolatableBuffer provides an easy way to estimate past measurements. One application 014 * might be in conjunction with the DifferentialDrivePoseEstimator, where knowledge of the robot 015 * pose at the time when vision or other global measurement were recorded is necessary, or for 016 * recording the past angles of mechanisms as measured by encoders. 017 * 018 * @param <T> The type stored in this buffer. 019 */ 020public final class TimeInterpolatableBuffer<T> { 021 private final double m_historySize; 022 private final Interpolator<T> m_interpolatingFunc; 023 private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>(); 024 025 /** 026 * Constructs a TimeInterpolatableBuffer. 027 * 028 * @param interpolateFunction Interpolation function. 029 * @param historySize The history size of the buffer in seconds. 030 */ 031 private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySize) { 032 this.m_historySize = historySize; 033 this.m_interpolatingFunc = interpolateFunction; 034 } 035 036 /** 037 * Create a new TimeInterpolatableBuffer. 038 * 039 * @param interpolateFunction The function used to interpolate between values. 040 * @param historySize The history size of the buffer in seconds. 041 * @param <T> The type of data to store in the buffer. 042 * @return The new TimeInterpolatableBuffer. 043 */ 044 public static <T> TimeInterpolatableBuffer<T> createBuffer( 045 Interpolator<T> interpolateFunction, double historySize) { 046 return new TimeInterpolatableBuffer<>(interpolateFunction, historySize); 047 } 048 049 /** 050 * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}. 051 * 052 * @param historySize The history size of the buffer in seconds. 053 * @param <T> The type of {@link Interpolatable} to store in the buffer. 054 * @return The new TimeInterpolatableBuffer. 055 */ 056 public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer( 057 double historySize) { 058 return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize); 059 } 060 061 /** 062 * Create a new TimeInterpolatableBuffer to store Double values. 063 * 064 * @param historySize The history size of the buffer in seconds. 065 * @return The new TimeInterpolatableBuffer. 066 */ 067 public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) { 068 return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySize); 069 } 070 071 /** 072 * Add a sample to the buffer. 073 * 074 * @param time The timestamp of the sample in seconds. 075 * @param sample The sample object. 076 */ 077 public void addSample(double time, T sample) { 078 cleanUp(time); 079 m_pastSnapshots.put(time, sample); 080 } 081 082 /** 083 * Removes samples older than our current history size. 084 * 085 * @param time The current timestamp in seconds. 086 */ 087 private void cleanUp(double time) { 088 while (!m_pastSnapshots.isEmpty()) { 089 var entry = m_pastSnapshots.firstEntry(); 090 if (time - entry.getKey() >= m_historySize) { 091 m_pastSnapshots.remove(entry.getKey()); 092 } else { 093 return; 094 } 095 } 096 } 097 098 /** Clear all old samples. */ 099 public void clear() { 100 m_pastSnapshots.clear(); 101 } 102 103 /** 104 * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned. 105 * 106 * @param time The time at which to sample in seconds. 107 * @return The interpolated value at that timestamp or an empty Optional. 108 */ 109 public Optional<T> getSample(double time) { 110 if (m_pastSnapshots.isEmpty()) { 111 return Optional.empty(); 112 } 113 114 // Special case for when the requested time is the same as a sample 115 var nowEntry = m_pastSnapshots.get(time); 116 if (nowEntry != null) { 117 return Optional.of(nowEntry); 118 } 119 120 var topBound = m_pastSnapshots.ceilingEntry(time); 121 var bottomBound = m_pastSnapshots.floorEntry(time); 122 123 // Return null if neither sample exists, and the opposite bound if the other is null 124 if (topBound == null && bottomBound == null) { 125 return Optional.empty(); 126 } else if (topBound == null) { 127 return Optional.of(bottomBound.getValue()); 128 } else if (bottomBound == null) { 129 return Optional.of(topBound.getValue()); 130 } else { 131 // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of (the difference 132 // between the current time and bottom bound) and (the difference between top and bottom 133 // bounds). 134 return Optional.of( 135 m_interpolatingFunc.interpolate( 136 bottomBound.getValue(), 137 topBound.getValue(), 138 (time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))); 139 } 140 } 141 142 /** 143 * Grant access to the internal sample buffer. Used in Pose Estimation to replay odometry inputs 144 * stored within this buffer. 145 * 146 * @return The internal sample buffer. 147 */ 148 public NavigableMap<Double, T> getInternalBuffer() { 149 return m_pastSnapshots; 150 } 151}