Added the calibration mode to the robobin.

ros2/src/robobin/robobin/uwb_navigation_node.py

@@ -7,7 +7,8 @@ import rclpy
 from rclpy.node import Node
 from std_msgs.msg import String
 from sensor_msgs.msg import Imu
-# from transforms3d.euler import quat2euler
+from tf.transformations import euler_from_quaternion
+
 import math
 
 class SerialController:
@@ -16,6 +17,12 @@ class SerialController:
         self.windowSize = windowSize
         self.minSuccessfulValues = minSuccessfulValues
         self.maxRetries = maxRetries
+        self.calibration_active = False
+        self.calibration_timer = None
+        self.calib_start_position = None
+        self.calib_start_yaw = None
+
+
 
         self.tag1Window = [[] for _ in range(4)]
         self.tag2Window = [[] for _ in range(4)]
@@ -111,10 +118,10 @@ class UWBNode(Node):
         self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
 
         self.subscription = self.create_subscription(String, '/nav_command', self.handle_nav_command, 10)
-        # self.imu_subscription = self.create_subscription(Imu, '/imu', self.handle_imu, 10)
+        self.imu_subscription = self.create_subscription(Imu, '/imu', self.handle_imu, 10)
 
-        self.current_yaw = 0.0  # Store the current IMU yaw
-        self.uwb_to_imu_offset = 0.0  # Offset between UWB and IMU heading
+        self.current_yaw = 0.0 
+        self.uwb_to_imu_offset = 0.0 
         self.mode = "Manual"
         self.serial_controller = SerialController("/dev/ttyACM0")
         self.anchors = {}
@@ -207,13 +214,22 @@ class UWBNode(Node):
             self.get_logger().error(f"Failed to determine anchors: {e}")
 
     def handle_imu(self, msg):
-        # Extract yaw from quaternion using transforms3d
-        orientation_q = msg.orientation
-        orientation_list = [orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z] 
-        # roll, pitch, yaw = quat2euler(orientation_list)  # Converts quaternion to Euler angles
+        # Extract the quaternion from the IMU message
+        qx = msg.orientation.x
+        qy = msg.orientation.y
+        qz = msg.orientation.z
+        qw = msg.orientation.w
+
+        # Convert quaternion to Euler angles (roll, pitch, yaw)
+        # euler_from_quaternion expects [x, y, z, w]
+        (roll, pitch, yaw) = euler_from_quaternion([qx, qy, qz, qw])
+
+        # Store yaw (in radians) for usage in your node
+        self.current_yaw = yaw
+
+        # Optional: Print or log if you want to see the values
+        self.get_logger().info(f"IMU yaw (rad): {yaw:.3f}")
 
-        self.current_yaw = 0  # Update current yaw
-        # self.get_logger().info(f"IMU Yaw: {yaw:.2f} radians")
 
     def determine_anchor_coords(self, measured_distances):
         y_A = measured_distances[2]  # Distance from A to D
@@ -449,20 +465,20 @@ class UWBNode(Node):
                 self.cmd_pub.publish(stop_cmd)  # Publish zero velocity
                 self.get_logger().info("Motors stopped.")
                     
-                if mode in ["Manual", "Follow", "Call"]:
+                if mode in ["Manual", "Follow", "Call","Calibration"]:
                     self.mode = mode
                     self.get_logger().info(f"Mode set to: {mode}")
 
-                    # Stop publishing to the pathing node when switching modes
+                    
                     if mode == "Manual":
-                        self.call_mode_active = False  # Disable call mode
+                        self.call_mode_active = False 
                         self.follow_offset_computed = False
                         self.follow_target_reached = False
                         self.get_logger().info("Publishing to pathing node stopped.")
                         
-                        # Stop any movement commands
+                       
                         stop_cmd = Twist()
-                        self.cmd_pub.publish(stop_cmd)  # Publish zero velocity
+                        self.cmd_pub.publish(stop_cmd)  
                         self.get_logger().info("Motors stopped.")
                         
                     elif mode == "Follow":
@@ -473,6 +489,9 @@ class UWBNode(Node):
                     elif mode == "Call":
                         self.call_mode_active = False
                         self.get_logger().info("Call mode initialized but not active yet.")
+                    elif mode == "Calibration":
+                        self.get_logger().info("Calibration mode initialized.")
+                        self.start_calibration()
                 else:
                     self.get_logger().error("Invalid mode for UWB Navigation")
 
@@ -487,7 +506,7 @@ class UWBNode(Node):
             end_location = parts[2]
             self.get_logger().info(f"Call received: Start={start_location}, End={end_location}")
             # Load graph
-            workspace_root = os.getcwd()  # Current working directory
+            workspace_root = os.getcwd()
             graph_file_path = os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "graph.json"))
             try:
                 with open(graph_file_path, 'r') as f:
@@ -503,7 +522,7 @@ class UWBNode(Node):
                 self.get_logger().error(f"Error: {str(e)}")
                 return
 
-            # Run A* to find path
+            
             self.path = self.a_star_search(self.graph_data, start_location, end_location)
             if self.path:
                 self.get_logger().info(f"Path found: {self.path}")
@@ -583,6 +602,66 @@ class UWBNode(Node):
             total_path.insert(0, nodes[current]["name"])
         return total_path
 
+    def start_calibration(self):
+        """
+        When doing calibration, the robobin will drive forward for 2 seconds at 0.25 m/s (WARNING).
+        """
+        if not self.current_tag1_position:
+            self.get_logger().error("No current UWB position available - cannot calibrate.")
+            return
+
+        self.calibration_active = True
+
+      
+        self.calib_start_position = self.current_tag1_position
+        self.calib_start_yaw = self.current_yaw
+  
+        forward_cmd = Twist()
+        forward_cmd.linear.x = 0.25
+        self.cmd_pub.publish(forward_cmd)
+        self.get_logger().info("Driving forward at 0.25 m/s for 2 seconds...")
+
+    
+        self.calibration_timer = self.create_timer(2.0, self.finish_calibration)
+
+    def finish_calibration(self):
+    
+     
+        if self.calibration_timer:
+            self.destroy_timer(self.calibration_timer)
+            self.calibration_timer = None
+
+        stop_cmd = Twist()
+        self.cmd_pub.publish(stop_cmd)
+
+        if not self.current_tag1_position:
+            self.get_logger().error("No final UWB position available - calibration failed.")
+            self.calibration_active = False
+            return
+
+        calib_end_position = self.current_tag1_position
+        calib_end_yaw = self.current_yaw  
+
+   
+        dx = calib_end_position[0] - self.calib_start_position[0]
+        dy = calib_end_position[1] - self.calib_start_position[1]
+        uwb_heading = math.atan2(dy, dx)
+
+        offset = calib_end_yaw - uwb_heading
+        self.uwb_to_imu_offset = offset
+
+
+        self.get_logger().info("CALIBRATION COMPLETE!")
+        self.get_logger().info(f" Start Pos (UWB):   {self.calib_start_position}")
+        self.get_logger().info(f" End Pos (UWB):     {calib_end_position}")
+        self.get_logger().info(f" UWB heading:       {uwb_heading:.3f} rad")
+        self.get_logger().info(f" IMU yaw:           {calib_end_yaw:.3f} rad")
+        self.get_logger().info(f" --> OFFSET:        {offset:.3f} rad")
+
+        
+        self.calibration_active = False
+        self.mode = "Manual" 
+
 def main(args=None):
     rclpy.init(args=args)
     node = UWBNode()