@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; }
private void sendCorrectionUpdatePacket() { errorBetweenCorrectedAndLocalizationTransform.getTranslation(translationalResidualError); totalErrorBetweenPelvisAndLocalizationTransform.getTranslation(translationalTotalError); double absoluteResidualError = translationalResidualError.length(); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteResidualError, (float) absoluteTotalError, hasMapBeenReset); hasMapBeenReset = false; pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
/** * pulls the corrected pose from the buffer, check that the nonprocessed buffer has * corresponding pelvis poses and calculates the total error */ private void processNewPacket() { StampedPosePacket newPacket = pelvisPoseCorrectionCommunicator.getNewExternalPose(); TimeStampedTransform3D timeStampedExternalPose = newPacket.getTransform(); if (stateEstimatorPelvisPoseBuffer.isInRange(timeStampedExternalPose.getTimeStamp())) { double confidence = newPacket.getConfidenceFactor(); confidence = MathTools.clipToMinMax(confidence, 0.0, 1.0); confidenceFactor.set(confidence); addNewExternalPose(timeStampedExternalPose); } }
private void sendCorrectionUpdatePacket() { totalRotationErrorFrame.get(totalRotationError); totalTranslationErrorFrame.get(totalTranslationError); totalError.set(totalRotationError, totalTranslationError); errorBetweenCurrentPositionAndCorrected.getTranslation(translationalResidualError); double absoluteResidualError = translationalResidualError.length(); totalError.getTranslation(translationalTotalError); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteTotalError, (float) absoluteResidualError, false); pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
StampedPosePacket newPacket = pelvisPoseCorrectionCommunicator.getNewExternalPose(); if (enableProcessNewPackets.getBooleanValue())
private void setupStateEstimationThread() { String robotName = robotModel.get().getSimpleRobotName(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator; if (externalPelvisCorrectorSubscriber.hasValue()) { pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get(); } else { pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator); ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator, s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData())); } stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter, yoVariableServer, gravity.get()); }
/** * poll for new packet, input for unit tests and real robot */ private void checkForNewPacket() { if (pelvisPoseCorrectionCommunicator.hasNewPose()) { processNewPacket(); sendCorrectionUpdate = true; } }