/** * Puts the stateEstimatorTransform in the buffer with the corresponding timeStamp * @param stateEstimatorTransform * @param timeStamp */ public void putStateEstimatorTransformInBuffer(RigidBodyTransform stateEstimatorTransform, long timeStamp) { stateEstimatorTimeStampedTransformBuffer.put(stateEstimatorTransform, timeStamp); }
/** * @param timeStamp * @return true if the timeStamp is in range of the stateEstimatorTimeStampedBuffer */ public boolean stateEstimatorTimeStampedBufferIsInRange(long timeStamp) { return stateEstimatorTimeStampedTransformBuffer.isInRange(timeStamp); }
public void getStateEstimatorTransform(long timestamp, TimeStampedTransform3D timeStampedTransform3DToPack) { stateEstimatorTimeStampedTransformBuffer.findTransform(timestamp, timeStampedTransform3DToPack); }
public HumanoidReferenceFrames getUpdatedReferenceFramesCopy() { updateRobotModel(); HumanoidReferenceFrames ret = new HumanoidReferenceFrames(fullRobotModel); return ret; }
private void callListeners() { DrillDetectionPacket newDrillPacket = incomingDetectedDrillPackets.poll(); if (newDrillPacket != null) { for (DrillDetectionStatusListener listener : listOfListeners) { listener.updateStatusPacket(newDrillPacket); } } }
public long getStateEstimatorTimeStampedBufferNewestTimestamp() { return stateEstimatorTimeStampedTransformBuffer.getNewestTimestamp(); }
public long getStateEstimatorTimeStampedBufferOldestTimestamp() { return stateEstimatorTimeStampedTransformBuffer.getOldestTimestamp(); }
@Override default void onNewDataMessage(Subscriber<StampedPosePacket> subscriber) { receivedPacket(subscriber.takeNextData()); } }
/** * poll for new packet, input for unit tests and real robot */ private void checkForNewPacket() { if (pelvisPoseCorrectionCommunicator.hasNewPose()) { processNewPacket(); sendCorrectionUpdate = true; } }
private void requestLocalizationReset() { pelvisPoseCorrectionCommunicator.sendLocalizationResetRequest(new LocalizationPacket(true, true)); hasMapBeenReset = true; }
@Override public void onNewDataMessage(Subscriber<HandDesiredConfigurationMessage> subscriber) { receivedPacket(subscriber.takeNextData()); }
@Override public void onNewDataMessage(Subscriber<StampedPosePacket> subscriber) { receivedPacket(subscriber.takeNextData()); }
@Override public void onNewDataMessage(Subscriber<ValkyrieHandFingerTrajectoryMessage> subscriber) { receivedPacket(subscriber.takeNextData()); }
public RigidBodyTransform getCurrentTestFrameTransformToWorld() { robotDataReceiver.updateRobotModel(); frameToKeepTrackOf.getTransformToDesiredFrame(currentTransformToWorld, worldFrame); return currentTransformToWorld; }
/** * adds noncorrected pelvis poses to buffer for pelvis pose lookups in past * @param pelvisPose non-corrected pelvis pose * @param timeStamp robot timestamp of pelvis pose */ private void addPelvisePoseToPelvisBuffer(RigidBodyTransform pelvisPose, long timeStamp) { seNonProcessedPelvisTimeStamp.set(timeStamp); stateEstimatorPelvisPoseBuffer.put(pelvisPose, timeStamp); }
private void callListeners() { DrillDetectionPacket newDrillPacket = incomingDetectedDrillPackets.poll(); if (newDrillPacket != null) { for (DrillDetectionStatusListener listener : listOfListeners) { listener.updateStatusPacket(newDrillPacket); } } }
/** * poll for new packet, input for unit tests and real robot */ private void checkForNewPacket() { if (pelvisPoseCorrectionCommunicator.hasNewPose()) { processNewPacket(); sendCorrectionUpdate = true; } }
public HumanoidReferenceFrames getUpdatedReferenceFramesCopy() { updateRobotModel(); HumanoidReferenceFrames ret = new HumanoidReferenceFrames(fullRobotModel); return ret; }
public RigidBodyTransform getCurrentTestFrameTransformToWorld() { robotDataReceiver.updateRobotModel(); frameToKeepTrackOf.getTransformToDesiredFrame(currentTransformToWorld, worldFrame); return currentTransformToWorld; }
public void updateRobotModel() { yoTimeLastFullRobotModelUpdate.set(yoTimeRobot.getDoubleValue()); robotDataReceiver.updateRobotModel(); }