aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: add bio to headless

Browse Source 
NOTE: UNTESTED
This commit is contained in:
David
2025-09-29 11:34:35 -05:00
parent 644459ec71
commit 97fa17a4a8
1 changed files with 42 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
src/headless_pkg/src/headless_node.py
View File

@@ -15,7 +15,7 @@ import glob
from std_msgs.msg import String from std_msgs.msg import String
from geometry_msgs.msg import Twist 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 from ros2_interfaces_pkg.msg import CoreCtrlState
import pygame 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() os.environ["SDL_AUDIODRIVER"] = "dummy" # Force pygame to use a dummy audio driver before pygame.init()
core_stop_msg = CoreControl() CORE_STOP_MSG = CoreControl() # All zeros by default
core_stop_msg.left_stick = 0.0 CORE_STOP_TWIST_MSG = Twist() # "
core_stop_msg.right_stick = 0.0 ARM_STOP_MSG = ArmManual() # "
core_stop_msg.max_speed = 0 BIO_STOP_MSG = BioControl() # "
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
control_qos = qos.QoSProfile( control_qos = qos.QoSProfile(
history=qos.QoSHistoryPolicy.KEEP_LAST, history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -67,7 +48,7 @@ class Headless(Node):
# Initialize pygame first # Initialize pygame first
pygame.init() pygame.init()
pygame.joystick.init() pygame.joystick.init()
# Wait for a gamepad to be connected # Wait for a gamepad to be connected
self.gamepad = None self.gamepad = None
print("Waiting for gamepad connection...") print("Waiting for gamepad connection...")
@@ -79,7 +60,7 @@ class Headless(Node):
sys.exit(0) sys.exit(0)
time.sleep(1.0) # Check every second time.sleep(1.0) # Check every second
print("No gamepad found. Waiting...") print("No gamepad found. Waiting...")
# Initialize the gamepad # Initialize the gamepad
self.gamepad = pygame.joystick.Joystick(0) self.gamepad = pygame.joystick.Joystick(0)
self.gamepad.init() self.gamepad.init()
@@ -88,11 +69,15 @@ class Headless(Node):
# Now initialize the ROS2 node # Now initialize the ROS2 node
super().__init__("headless") super().__init__("headless")
self.create_timer(0.15, self.send_controls) 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_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.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
self.ctrl_mode = "core" # Start in core mode self.ctrl_mode = "core" # Start in core mode
self.core_brake_mode = False self.core_brake_mode = False
self.core_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast self.core_speed_mode = 0 # -1 = slow, 0 = walk, 1 = fast
@@ -123,8 +108,9 @@ class Headless(Node):
if pygame.joystick.get_count() == 0: if pygame.joystick.get_count() == 0:
print("Gamepad disconnected. Exiting...") print("Gamepad disconnected. Exiting...")
# Send one last zero control message # Send one last zero control message
self.core_publisher.publish(core_stop_msg) self.core_publisher.publish(CORE_STOP_MSG)
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("Final stop commands sent. Shutting down.") self.get_logger().info("Final stop commands sent. Shutting down.")
# Clean up # Clean up
pygame.quit() pygame.quit()
@@ -176,7 +162,8 @@ class Headless(Node):
self.get_logger().info(f"[Ctrl] {output}") self.get_logger().info(f"[Ctrl] {output}")
self.core_publisher.publish(input) 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": elif self.ctrl_mode == "core" and CORE_MODE == "twist":
input = Twist() input = Twist()
@@ -194,7 +181,8 @@ class Headless(Node):
# Publish # Publish
self.core_twist_pub_.publish(input) 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)}") self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}")
# Brake mode # 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}") 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": if self.ctrl_mode == "arm":
input = ArmManual() arm_input = ArmManual()
# Collect controller state # Collect controller state
left_stick_x = deadzone(self.gamepad.get_axis(0)) left_stick_x = deadzone(self.gamepad.get_axis(0))
@@ -235,49 +223,54 @@ class Headless(Node):
# EF Grippers # EF Grippers
if left_trigger > 0 and right_trigger > 0: if left_trigger > 0 and right_trigger > 0:
input.gripper = 0 arm_input.gripper = 0
elif left_trigger > 0: elif left_trigger > 0:
input.gripper = -1 arm_input.gripper = -1
elif right_trigger > 0: elif right_trigger > 0:
input.gripper = 1 arm_input.gripper = 1
# Axis 0 # Axis 0
if dpad_input[0] == 1: if dpad_input[0] == 1:
input.axis0 = 1 arm_input.axis0 = 1
elif dpad_input[0] == -1: elif dpad_input[0] == -1:
input.axis0 = -1 arm_input.axis0 = -1
if right_bumper: # Control end effector if right_bumper: # Control end effector
# Effector yaw # Effector yaw
if left_stick_x > 0: if left_stick_x > 0:
input.effector_yaw = 1 arm_input.effector_yaw = 1
elif left_stick_x < 0: elif left_stick_x < 0:
input.effector_yaw = -1 arm_input.effector_yaw = -1
# Effector roll # Effector roll
if right_stick_x > 0: if right_stick_x > 0:
input.effector_roll = 1 arm_input.effector_roll = 1
elif right_stick_x < 0: elif right_stick_x < 0:
input.effector_roll = -1 arm_input.effector_roll = -1
else: # Control arm axis else: # Control arm axis
# Axis 1 # Axis 1
if abs(left_stick_x) > .15: if abs(left_stick_x) > .15:
input.axis1 = round(left_stick_x) arm_input.axis1 = round(left_stick_x)
# Axis 2 # Axis 2
if abs(left_stick_y) > .15: if abs(left_stick_y) > .15:
input.axis2 = -1 * round(left_stick_y) arm_input.axis2 = -1 * round(left_stick_y)
# Axis 3 # Axis 3
if abs(right_stick_y) > .15: 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: def deadzone(value: float, threshold=0.05) -> float: