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

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

@@ -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,
@@ -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: