added offset for gps to center of robot

LegoMaster7001 · LegoMaster7001 · commit 539423b3c5fa · 2026-01-16T09:01:49.000-06:00
diff --git a/src/jetson_pkg/jetson_pkg/gps_apriltag_comparison.py b/src/jetson_pkg/jetson_pkg/gps_apriltag_comparison.py
@@ -50,8 +50,13 @@ def coords_distance(self, apriltag_coords: Twist, gps_coords: Twist):
         apriltagX = apriltag_coords.linear.x
         apriltagY = apriltag_coords.linear.y
 
-        gpsX = gps_coords.linear.x
-        gpsY = gps_coords.linear.y
+        
+        gpsX = gps_coords.linear.x - math.cos(apriltag_coords.angular.y) * 0.39
+        gpsY = gps_coords.linear.y - math.sin(apriltag_coords.angular.y) * 0.39
+
+
+
+
 
         #turns the x and y into a circle
         distance = math.sqrt((apriltagX - gpsX)**2 + (apriltagY - gpsY)**2)
diff --git a/src/jetson_pkg/jetson_pkg/gps_conversion.py b/src/jetson_pkg/jetson_pkg/gps_conversion.py
diff --git a/src/jetson_pkg/jetson_pkg/jetson_apriltag.py b/src/jetson_pkg/jetson_pkg/jetson_apriltag.py
@@ -78,6 +78,7 @@ def timer_callback(self):
                 msg_no_offset.linear.x = relative_x
                 msg_no_offset.linear.z = relative_z
                 msg_no_offset.linear.y = relative_y
+                msg_no_offset.angular.y = thetar
                 
                 self.get_logger().debug(f'{msg_no_offset}')
                 self.publisher_.publish(msg_no_offset)
