Fix for odometry node

art-e-fact · Feb 13, 2025 · f79e2ff · f79e2ff
1 parent 1c0e0c7
commit f79e2ff
Showing 1 changed file with 7 additions and 6 deletions.
diff --git a/src/sam_bot_nav2_gz/test/test_odometry_node.py b/src/sam_bot_nav2_gz/test/test_odometry_node.py
@@ -81,12 +81,13 @@ def odom_callback(self, msg: Odometry):
             #self.get_logger().info(f"Distance from start: {distance_from_start_est}")
 
             # compute the position estimate error
-            odometry_error = math.sqrt(
-                (self.current_location.x - msg.pose.pose.position.x) ** 2
-                + (self.current_location.y - msg.pose.pose.position.y) ** 2
-            )
-            # publish the cumulative error
-            self.odometry_error_pub.publish(Float32(data=odometry_error))
+            if self.current_location:
+                odometry_error = math.sqrt(
+                    (self.current_location.x - msg.pose.pose.position.x) ** 2
+                    + (self.current_location.y - msg.pose.pose.position.y) ** 2
+                )
+                # publish the cumulative error
+                self.odometry_error_pub.publish(Float32(data=odometry_error))
 
 
         self.last_odom_msg = msg