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

reformat with black

Browse Source
This commit is contained in:
ryleu
2025-11-06 19:10:21 -06:00
parent b670bc2eda
commit c107b82a8d
15 changed files with 686 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/headless_pkg/setup.py
View File

@@ -1,23 +1,23 @@
from setuptools import find_packages, setup
package_name = 'headless_pkg'
package_name = "headless_pkg"
setup(
name=package_name,
version='1.0.0',
packages=find_packages(exclude=['test']),
version="1.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=['setuptools'],
install_requires=["setuptools"],
zip_safe=True,
maintainer='David Sharpe',
maintainer_email='ds0196@uah.edu',
description='Headless rover control package to handle command interpretation and embedded interfacing.',
license='All Rights Reserved',
maintainer="David Sharpe",
maintainer_email="ds0196@uah.edu",
description="Headless rover control package to handle command interpretation and embedded interfacing.",
license="All Rights Reserved",
entry_points={
'console_scripts': [
"console_scripts": [
"headless_full = src.headless_node:main",
],
},

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

@@ -21,13 +21,15 @@ from ros2_interfaces_pkg.msg import CoreCtrlState
import pygame
os.environ["SDL_VIDEODRIVER"] = "dummy" # Prevents pygame from trying to open a display
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() # All zeros by default
CORE_STOP_TWIST_MSG = Twist() # "
ARM_STOP_MSG = ArmManual() # "
BIO_STOP_MSG = BioControl() # "
ARM_STOP_MSG = ArmManual() # "
BIO_STOP_MSG = BioControl() # "
control_qos = qos.QoSProfile(
history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -37,7 +39,7 @@ control_qos = qos.QoSProfile(
deadline=Duration(seconds=1),
lifespan=Duration(nanoseconds=500_000_000), # 500ms
liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
liveliness_lease_duration=Duration(seconds=5)
liveliness_lease_duration=Duration(seconds=5),
)
CORE_MODE = "twist" # "twist" or "duty"
@@ -51,11 +53,11 @@ class Headless(Node):
super().__init__("headless")
# Wait for anchor to start
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
while len(pub_info) == 0:
self.get_logger().info("Waiting for anchor to start...")
time.sleep(1.0)
pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
# Wait for a gamepad to be connected
print("Waiting for gamepad connection...")
@@ -71,17 +73,20 @@ class Headless(Node):
# Initialize the gamepad
self.gamepad = pygame.joystick.Joystick(0)
self.gamepad.init()
print(f'Gamepad Found: {self.gamepad.get_name()}')
print(f"Gamepad Found: {self.gamepad.get_name()}")
self.create_timer(0.15, self.send_controls)
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.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
@@ -90,12 +95,11 @@ class Headless(Node):
# Rumble when node is ready (returns False if rumble not supported)
self.gamepad.rumble(0.7, 0.8, 150)
def run(self):
# This thread makes all the update processes run in the background
thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
thread.start()
try:
while rclpy.ok():
self.send_controls()
@@ -104,12 +108,12 @@ class Headless(Node):
sys.exit(0)
def send_controls(self):
""" Read the gamepad state and publish control messages """
"""Read the gamepad state and publish control messages"""
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit(0)
# Check if controller is still connected
if pygame.joystick.get_count() == 0:
print("Gamepad disconnected. Exiting...")
@@ -122,7 +126,6 @@ class Headless(Node):
pygame.quit()
sys.exit(0)
new_ctrl_mode = self.ctrl_mode # if "" then inequality will always be true
# Check for control mode change
@@ -137,7 +140,6 @@ class Headless(Node):
self.ctrl_mode = new_ctrl_mode
self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
# CORE
if self.ctrl_mode == "core" and CORE_MODE == "duty":
input = CoreControl()
@@ -148,7 +150,6 @@ class Headless(Node):
right_stick_y = deadzone(self.gamepad.get_axis(4))
right_trigger = deadzone(self.gamepad.get_axis(5))
# Right wheels
input.right_stick = float(round(-1 * right_stick_y, 2))
@@ -158,15 +159,14 @@ class Headless(Node):
else:
input.left_stick = float(round(-1 * left_stick_y, 2))
# Debug
output = f'L: {input.left_stick}, R: {input.right_stick}'
output = f"L: {input.left_stick}, R: {input.right_stick}"
self.get_logger().info(f"[Ctrl] {output}")
self.core_publisher.publish(input)
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()
@@ -179,13 +179,17 @@ class Headless(Node):
# Forward/back and Turn
input.linear.x = -1.0 * left_stick_y
input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x) # Exponent for finer control (curve)
input.angular.z = -1.0 * copysign(
right_stick_x**2, right_stick_x
) # Exponent for finer control (curve)
# Publish
self.core_twist_pub_.publish(input)
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
new_brake_mode = button_a
@@ -198,20 +202,24 @@ class Headless(Node):
new_max_duty = 0.5
# Only publish if needed
if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
if (
new_brake_mode != self.core_brake_mode
or new_max_duty != self.core_max_duty
):
self.core_brake_mode = new_brake_mode
self.core_max_duty = new_max_duty
state_msg = CoreCtrlState()
state_msg.brake_mode = bool(self.core_brake_mode)
state_msg.max_duty = float(self.core_max_duty)
self.core_state_pub_.publish(state_msg)
self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
self.get_logger().info(
f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
)
# ARM and BIO
if self.ctrl_mode == "arm":
arm_input = ArmManual()
# Collect controller state
left_stick_x = deadzone(self.gamepad.get_axis(0))
left_stick_y = deadzone(self.gamepad.get_axis(1))
@@ -222,7 +230,6 @@ class Headless(Node):
right_bumper = self.gamepad.get_button(5)
dpad_input = self.gamepad.get_hat(0)
# EF Grippers
if left_trigger > 0 and right_trigger > 0:
arm_input.gripper = 0
@@ -237,7 +244,6 @@ class Headless(Node):
elif dpad_input[0] == -1:
arm_input.axis0 = -1
if right_bumper: # Control end effector
# Effector yaw
@@ -252,30 +258,31 @@ class Headless(Node):
elif right_stick_x < 0:
arm_input.effector_roll = -1
else: # Control arm axis
else: # Control arm axis
# Axis 1
if abs(left_stick_x) > .15:
if abs(left_stick_x) > 0.15:
arm_input.axis1 = round(left_stick_x)
# Axis 2
if abs(left_stick_y) > .15:
if abs(left_stick_y) > 0.15:
arm_input.axis2 = -1 * round(left_stick_y)
# Axis 3
if abs(right_stick_y) > .15:
if abs(right_stick_y) > 0.15:
arm_input.axis3 = -1 * round(right_stick_y)
# BIO
bio_input = BioControl(
bio_arm=int(left_stick_y * -100),
drill_arm=int(round(right_stick_y) * -100)
drill_arm=int(round(right_stick_y) * -100),
)
# Drill motor (FAERIE)
if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
bio_input.drill = int(30 * (right_trigger - left_trigger)) # Max duty cycle 30%
bio_input.drill = int(
30 * (right_trigger - left_trigger)
) # Max duty cycle 30%
self.core_publisher.publish(CORE_STOP_MSG)
self.arm_publisher.publish(arm_input)
@@ -283,7 +290,7 @@ class Headless(Node):
def deadzone(value: float, threshold=0.05) -> float:
""" Apply a deadzone to a joystick input so the motors don't sound angry """
"""Apply a deadzone to a joystick input so the motors don't sound angry"""
if abs(value) < threshold:
return 0
return value
@@ -295,6 +302,9 @@ def main(args=None):
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0)) # Catch termination signals and exit cleanly
if __name__ == "__main__":
signal.signal(
signal.SIGTERM, lambda signum, frame: sys.exit(0)
) # Catch termination signals and exit cleanly
main()