@Override public void write(long timestamp) { try { long startTimestamp = estimatorTime.getLongValue(); threadDataSynchronizer.publishEstimatorState(startTimestamp, estimatorTick.getLongValue(), startClockTime.getLongValue()); if (robotVisualizer != null) { robotVisualizer.update(startTimestamp); } estimatorTick.increment(); rootFrame.getTransformToDesiredFrame(rootToWorldTransform, ReferenceFrame.getWorldFrame()); yoGraphicsListRegistry.setControllerTransformToWorld(rootToWorldTransform); } catch (Throwable e) { if (globalDataProducer != null) { globalDataProducer.notifyControllerCrash(CrashLocation.ESTIMATOR_WRITE, e.getMessage()); } throw new RuntimeException(e); } }
@Override public void write(long timestamp) { try { if (runController.getBooleanValue()) { outputWriter.writeAfterController(TimeTools.secondsToNanoSeconds(controllerTime.getDoubleValue())); totalDelay.set(timestamp - lastEstimatorStartTime.getLongValue()); threadDataSynchronizer.publishControllerData(); if (robotVisualizer != null) { robotVisualizer.update(TimeTools.secondsToNanoSeconds(controllerTime.getDoubleValue()), registry); } rootFrame.getTransformToDesiredFrame(rootToWorldTransform, ReferenceFrame.getWorldFrame()); yoGraphicsListRegistry.setControllerTransformToWorld(rootToWorldTransform); } } catch (Exception e) { globalDataProducer.notifyControllerCrash(CrashLocation.CONTROLLER_WRITE, e.getMessage()); throw new RuntimeException(e); } }
yoGraphicsListRegistry.setControllerTransformToWorld(rootToWorldTransform);
@Override public void write(long timestamp) { try { if (runController.getBooleanValue()) { if (outputProcessor != null) { outputProcessor.processAfterController(controllerTimestamp.getLongValue()); } totalDelay.set(timestamp - lastEstimatorStartTime.getLongValue()); threadDataSynchronizer.publishControllerData(); if (robotVisualizer != null) { robotVisualizerUpdateTimer.startMeasurement(); robotVisualizer.update(controllerTimestamp.getLongValue(), registry); robotVisualizerUpdateTimer.stopMeasurement(); } rootFrame.getTransformToDesiredFrame(rootToWorldTransform, ReferenceFrame.getWorldFrame()); yoGraphicsListRegistry.setControllerTransformToWorld(rootToWorldTransform); } } catch (Exception e) { crashNotificationPublisher.publish(MessageTools.createControllerCrashNotificationPacket(ControllerCrashLocation.CONTROLLER_WRITE, e.getMessage())); throw new RuntimeException(e); } }