diff --git a/ros2/src/robobin/robobin/uwb_navigation_node.py b/ros2/src/robobin/robobin/uwb_navigation_node.py index 925b464de2d53f45eb35dbed6810ca952398cf80..21767f41fc4ca24e576017fa7aaf1132242df8ef 100644 --- a/ros2/src/robobin/robobin/uwb_navigation_node.py +++ b/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()