Fixed setPose

build.gradle

@@ -8,7 +8,7 @@ buildscript {
 
     }
     dependencies {
-        classpath 'com.android.tools.build:gradle:3.6.4'
+        classpath 'com.android.tools.build:gradle:7.0.4'
         classpath 'com.diffplug.spotless:spotless-plugin-gradle:3.28.0'
     }
 }

gradle/wrapper/gradle-wrapper.properties

@@ -3,4 +3,4 @@ distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.4-all.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.0.2-all.zip

lib/build.gradle

@@ -7,13 +7,13 @@ sourceCompatibility = JavaVersion.VERSION_1_8
 targetCompatibility = JavaVersion.VERSION_1_8
 
 android {
-    compileSdkVersion 29
+    compileSdkVersion 30
     buildToolsVersion "29.0.3"
     ndkVersion "21.0.6113669"
 
     defaultConfig {
         minSdkVersion 24
-        targetSdkVersion 29
+        targetSdkVersion 30
         versionCode 14 // This number just needs to increase monotonically after every version change
         versionName "2.1.9"
 

lib/src/main/cpp/t265wrapper.cpp

@@ -223,7 +223,7 @@ Java_com_spartronics4915_lib_T265Camera_newCamera(JNIEnv *env, jobject thisObj,
         auto yaw = 2 * atan2f(poseData.rotation.y, poseData.rotation.w);
 
         auto callbackMethodID =
-            env->GetMethodID(holdingClass, "consumePoseUpdate", "(FFFFFFI)V");
+            env->GetMethodID(holdingClass, "consumeCameraUpdate", "(FFFFFFI)V");
         if (!callbackMethodID)
           throw std::runtime_error("consumePoseUpdate method doesn't exist");
 

lib/src/main/java/com/spartronics4915/lib/T265Camera.java

@@ -87,9 +87,18 @@ public CameraUpdate(Pose2d pose, ChassisSpeeds velocity, PoseConfidence confiden
 
     // Protected by mUpdateMutex
     private final Object mUpdateMutex = new Object();
+    private Pose2d mLastRecievedCameraUpdate = null;
     private CameraUpdate mLastRecievedUpdate = null;
     private Consumer<CameraUpdate> mPoseConsumer = null;
 
+    // Consumer for CameraUpdate
+    private final Consumer<Pose2d> cameraConsumer =
+            (update) -> {
+                synchronized (mUpdateMutex) {
+                    mLastRecievedCameraUpdate = update;
+                }
+            };
+
     /**
      * This method constructs a T265 camera and sets it up with the right info. {@link
      * T265Camera#start start} will not be called, you must call it yourself.
@@ -327,7 +336,9 @@ public synchronized void setPose(Pose2d newPose) {
         synchronized (mUpdateMutex) {
             mOriginOffset =
                     newPose.relativeTo(
-                            mLastRecievedUpdate == null ? new Pose2d() : mLastRecievedUpdate.pose);
+                            mLastRecievedCameraUpdate == null
+                                    ? new Pose2d()
+                                    : mLastRecievedCameraUpdate);
         }
     }
 
@@ -349,6 +360,21 @@ private native void setOdometryInfo(
 
     private static native void cleanup();
 
+    private synchronized void consumeCameraUpdate(
+            float x, float y, float radians, float dx, float dy, float dtheta, int confOrdinal) {
+        final Pose2d cameraUpdate =
+                new Pose2d(
+                                x - mRobotOffset.getTranslation().getX(),
+                                y - mRobotOffset.getTranslation().getY(),
+                                new Rotation2d(radians))
+                        .transformBy(mRobotOffset);
+
+        if (!mIsStarted) return;
+
+        cameraConsumer.accept(cameraUpdate);
+        consumePoseUpdate(x, y, radians, dx, dy, dtheta, confOrdinal);
+    }
+
     private synchronized void consumePoseUpdate(
             float x, float y, float radians, float dx, float dy, float dtheta, int confOrdinal) {
         // First we apply an offset to go from the camera coordinate system to the