From 97fa17a4a8ce7b7ab0d77677b2833d456b97fd18 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 29 Sep 2025 11:34:35 -0500 Subject: [PATCH] feat: add bio to headless NOTE: UNTESTED --- src/headless_pkg/src/headless_node.py | 91 +++++++++++++-------------- 1 file changed, 42 insertions(+), 49 deletions(-) diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index fc974f6..cc80f9f 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -15,7 +15,7 @@ import glob from std_msgs.msg import String from geometry_msgs.msg import Twist -from ros2_interfaces_pkg.msg import CoreControl, ArmManual +from ros2_interfaces_pkg.msg import CoreControl, ArmManual, BioControl from ros2_interfaces_pkg.msg import CoreCtrlState import pygame @@ -24,29 +24,10 @@ os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init() -core_stop_msg = CoreControl() -core_stop_msg.left_stick = 0.0 -core_stop_msg.right_stick = 0.0 -core_stop_msg.max_speed = 0 - -core_stop_twist_msg = Twist() -core_stop_twist_msg.linear.x = 0.0 -core_stop_twist_msg.linear.y = 0.0 -core_stop_twist_msg.linear.z = 0.0 -core_stop_twist_msg.angular.x = 0.0 -core_stop_twist_msg.angular.y = 0.0 -core_stop_twist_msg.angular.z = 0.0 - -arm_stop_msg = ArmManual() -arm_stop_msg.axis0 = 0 -arm_stop_msg.axis1 = 0 -arm_stop_msg.axis2 = 0 -arm_stop_msg.axis3 = 0 -arm_stop_msg.effector_roll = 0 -arm_stop_msg.effector_yaw = 0 -arm_stop_msg.gripper = 0 -arm_stop_msg.linear_actuator = 0 -arm_stop_msg.laser = 0 +CORE_STOP_MSG = CoreControl() # All zeros by default +CORE_STOP_TWIST_MSG = Twist() # " +ARM_STOP_MSG = ArmManual() # " +BIO_STOP_MSG = BioControl() # " control_qos = qos.QoSProfile( history=qos.QoSHistoryPolicy.KEEP_LAST, @@ -67,7 +48,7 @@ class Headless(Node): # Initialize pygame first pygame.init() pygame.joystick.init() - + # Wait for a gamepad to be connected self.gamepad = None print("Waiting for gamepad connection...") @@ -79,7 +60,7 @@ class Headless(Node): sys.exit(0) time.sleep(1.0) # Check every second print("No gamepad found. Waiting...") - + # Initialize the gamepad self.gamepad = pygame.joystick.Joystick(0) self.gamepad.init() @@ -88,11 +69,15 @@ class Headless(Node): # Now initialize the ROS2 node super().__init__("headless") self.create_timer(0.15, self.send_controls) - self.core_publisher = self.create_publisher(CoreControl, '/core/control', 10) - self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 10) + + self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2) + self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2) + self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2) + self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos) self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos) - + + self.ctrl_mode = "core" # Start in core mode self.core_brake_mode = False self.core_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast @@ -123,8 +108,9 @@ class Headless(Node): if pygame.joystick.get_count() == 0: print("Gamepad disconnected. Exiting...") # Send one last zero control message - self.core_publisher.publish(core_stop_msg) - self.arm_publisher.publish(arm_stop_msg) + self.core_publisher.publish(CORE_STOP_MSG) + self.arm_publisher.publish(ARM_STOP_MSG) + self.bio_publisher.publish(BIO_STOP_MSG) self.get_logger().info("Final stop commands sent. Shutting down.") # Clean up pygame.quit() @@ -176,7 +162,8 @@ class Headless(Node): self.get_logger().info(f"[Ctrl] {output}") self.core_publisher.publish(input) - self.arm_publisher.publish(arm_stop_msg) + self.arm_publisher.publish(ARM_STOP_MSG) + self.bio_publisher.publish(BIO_STOP_MSG) elif self.ctrl_mode == "core" and CORE_MODE == "twist": input = Twist() @@ -194,7 +181,8 @@ class Headless(Node): # Publish self.core_twist_pub_.publish(input) - self.arm_publisher.publish(arm_stop_msg) + self.arm_publisher.publish(ARM_STOP_MSG) + self.bio_publisher.publish(BIO_STOP_MSG) self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}") # Brake mode @@ -218,9 +206,9 @@ class Headless(Node): self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Speed: {self.core_speed_mode}") - # ARM + # ARM and BIO if self.ctrl_mode == "arm": - input = ArmManual() + arm_input = ArmManual() # Collect controller state left_stick_x = deadzone(self.gamepad.get_axis(0)) @@ -235,49 +223,54 @@ class Headless(Node): # EF Grippers if left_trigger > 0 and right_trigger > 0: - input.gripper = 0 + arm_input.gripper = 0 elif left_trigger > 0: - input.gripper = -1 + arm_input.gripper = -1 elif right_trigger > 0: - input.gripper = 1 + arm_input.gripper = 1 # Axis 0 if dpad_input[0] == 1: - input.axis0 = 1 + arm_input.axis0 = 1 elif dpad_input[0] == -1: - input.axis0 = -1 + arm_input.axis0 = -1 if right_bumper: # Control end effector # Effector yaw if left_stick_x > 0: - input.effector_yaw = 1 + arm_input.effector_yaw = 1 elif left_stick_x < 0: - input.effector_yaw = -1 + arm_input.effector_yaw = -1 # Effector roll if right_stick_x > 0: - input.effector_roll = 1 + arm_input.effector_roll = 1 elif right_stick_x < 0: - input.effector_roll = -1 + arm_input.effector_roll = -1 else: # Control arm axis # Axis 1 if abs(left_stick_x) > .15: - input.axis1 = round(left_stick_x) + arm_input.axis1 = round(left_stick_x) # Axis 2 if abs(left_stick_y) > .15: - input.axis2 = -1 * round(left_stick_y) + arm_input.axis2 = -1 * round(left_stick_y) # Axis 3 if abs(right_stick_y) > .15: - input.axis3 = -1 * round(right_stick_y) + arm_input.axis3 = -1 * round(right_stick_y) - self.core_publisher.publish(core_stop_msg) - self.arm_publisher.publish(input) + + # BIO + bio_input = BioControl(bio_arm=(left_stick_y * -100), drill_arm=(round(right_stick_y) * -100)) + + self.core_publisher.publish(CORE_STOP_MSG) + self.arm_publisher.publish(arm_input) + self.bio_publisher.publish(bio_input) def deadzone(value: float, threshold=0.05) -> float: