Source:
<code>public HighTrustGPSLocalizer(){ buggyFrameGpsX = 0.0; <START> buggyFrameGpsY = 0.0; <END> buggyFrameRotZ = 0.0; lastEncoderReading = 0.0; posePub = new Publisher(NodeChannel.POSE.getMsgPath()); mostRecentUpdate = new Date(); new Subscriber(NodeChannel.GPS.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { GpsMeasurement newGPSData = (GpsMeasurement)m; synchronized (this) { long dt = newGPSData.getTimestamp().getTime() - mostRecentUpdate.getTime(); if(dt > 0.0){ buggyFrameGpsY = newGPSData.getLatitude(); buggyFrameGpsX = newGPSData.getLongitude(); publishUpdate(); mostRecentUpdate = newGPSData.getTimestamp(); } } } }); new Subscriber(NodeChannel.IMU_ANG_POS.getMsgPath(), ((topicName, m) -> { IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); double y = mes.getRot()[0][1]; double x = mes.getRot()[0][0]; buggyFrameRotZ = Math.toDegrees(Math.atan2(y, x)); publishUpdate(); })); new Subscriber(NodeChannel.ENCODER.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { EncoderMeasurement measurement = (EncoderMeasurement) m; double currentEncoderMeasurement = measurement.getDistance(); double deltaDistance = currentEncoderMeasurement - lastEncoderReading; double deltaMeters = deltaDistance; LocTuple deltaPos = LocalizerUtil.convertMetersToLatLng(deltaMeters, buggyFrameRotZ); buggyFrameGpsY += deltaPos.getLatitude(); buggyFrameGpsX += deltaPos.getLongitude(); lastEncoderReading = currentEncoderMeasurement; publishUpdate(); } }); }</code><technical_language>Note subscriber callbacks run threads. hit races if plan for this,</technical_language>

Target:
public HighTrustGPSLocalizer(){ buggyFrameGpsX = 0.0; buggyFrameGpsY = 0.0; buggyFrameRotZ = 0.0; lastEncoderReading = 0.0; posePub = new Publisher(NodeChannel.POSE.getMsgPath()); mostRecentUpdate = new Date(); new Subscriber(NodeChannel.GPS.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { GpsMeasurement newGPSData = (GpsMeasurement)m; synchronized (this) { long dt = newGPSData.getTimestamp().getTime() - mostRecentUpdate.getTime(); if(dt > 0.0){ buggyFrameGpsY = newGPSData.getLatitude(); buggyFrameGpsX = newGPSData.getLongitude(); publishUpdate(); mostRecentUpdate = newGPSData.getTimestamp(); } } } }); new Subscriber(NodeChannel.IMU_ANG_POS.getMsgPath(), ((topicName, m) -> { IMUAngularPositionMessage mes = ((IMUAngularPositionMessage) m); double y = mes.getRot()[0][1]; double x = mes.getRot()[0][0]; buggyFrameRotZ = Util.normalizeAngleDeg(-Math.toDegrees(Math.atan2(y, x))+90); publishUpdate(); })); new Subscriber(NodeChannel.ENCODER.getMsgPath(), new MessageListener() { @Override public void actionPerformed(String topicName, Message m) { EncoderMeasurement measurement = (EncoderMeasurement) m; double currentEncoderMeasurement = measurement.getDistance(); double deltaDistance = currentEncoderMeasurement - lastEncoderReading; LocTuple deltaPos = LocalizerUtil.convertMetersToLatLng(deltaDistance, buggyFrameRotZ); buggyFrameGpsY += deltaPos.getLatitude(); buggyFrameGpsX += deltaPos.getLongitude(); lastEncoderReading = currentEncoderMeasurement; publishUpdate(); } }); }