externalPelvisPoseSubscriber = new PelvisPoseCorrectionCommunicator(null, null); ROS2Tools.createCallbackSubscription(estimatorRealtimeRos2Node, StampedPosePacket.class, ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName), externalPelvisPoseSubscriber);
private void setupStateEstimationThread() { stateEstimationThread = new DRCEstimatorThread(robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), humanoidGlobalDataProducer.get(), yoVariableServer, gravity.get()); if (humanoidGlobalDataProducer.get() != null) { PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(humanoidGlobalDataProducer.get()); humanoidGlobalDataProducer.get().attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator); stateEstimationThread.setExternalPelvisCorrectorSubscriber(pelvisPoseCorrectionCommunicator); } else { stateEstimationThread.setExternalPelvisCorrectorSubscriber(null); } }
PelvisPoseCorrectionCommunicatorInterface externalPelvisPoseSubscriber = new PelvisPoseCorrectionCommunicator(dataProducer); dataProducer.attachListener(StampedPosePacket.class, externalPelvisPoseSubscriber);
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()); }
pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(globalDataProducer); globalDataProducer.attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator);
PelvisPoseCorrectionCommunicatorInterface externalPelvisPoseSubscriber = new PelvisPoseCorrectionCommunicator(dataProducer); dataProducer.attachListener(StampedPosePacket.class, externalPelvisPoseSubscriber);
PelvisPoseCorrectionCommunicatorInterface externalPelvisPoseSubscriber = new PelvisPoseCorrectionCommunicator(dataProducer); dataProducer.attachListener(StampedPosePacket.class, externalPelvisPoseSubscriber);