public long getStateEstimatorTimeStampedBufferNewestTimestamp() { return stateEstimatorTimeStampedTransformBuffer.getNewestTimestamp(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testEmptyBuffer() { Random random = new Random(5616L); TimeStampedTransformBuffer timeStampedPelvisPoseBuffer = new TimeStampedTransformBuffer(5151); assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), 0L); assertEquals(timeStampedPelvisPoseBuffer.getOldestTimestamp(), Long.MAX_VALUE); for (int i = 0; i < 1000; i++) assertFalse(timeStampedPelvisPoseBuffer.isInRange(random.nextLong())); }
assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp);
assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp);
assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp); assertEquals(timeStampedPelvisPoseBuffer.getOldestTimestamp(), expectedOldestTimeStamp);
assertEquals(timeStampedPelvisPoseBuffer.getNewestTimestamp(), expectedNewestTimeStamp);
public void manuallyTriggerLocalizationUpdate() { confidenceFactor.set(1.0); RigidBodyTransform pelvisPose = new RigidBodyTransform(); Quat4d rotation = new Quat4d(); pelvisPose.getRotation(rotation); RotationTools.convertYawPitchRollToQuaternion(manualRotationOffsetInRadZ.getDoubleValue(), manualRotationOffsetInRadY.getDoubleValue(), manualRotationOffsetInRadX.getDoubleValue(), rotation); pelvisPose.setRotation(rotation); Vector3d translation = new Vector3d(); pelvisPose.getTranslation(translation); translation.setX(manualTranslationOffsetX.getDoubleValue()); translation.setY(manualTranslationOffsetY.getDoubleValue()); translation.setZ(manualTranslationOffsetZ.getDoubleValue()); pelvisPose.setTranslation(translation); TimeStampedTransform3D manualTimeStampedTransform3D = new TimeStampedTransform3D(pelvisPose, stateEstimatorPelvisPoseBuffer.getNewestTimestamp()); addNewExternalPose(manualTimeStampedTransform3D); manuallyTriggerLocalizationUpdate.set(false); }