aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-04-20 03:41:17 -05:00
Code Issues Packages Projects Releases Wiki Activity

refactor: (headless) change string parameters to bool

Browse Source
This commit is contained in:
David Sharpe
2026-03-16 00:39:22 -05:00
parent 292b3a742d
commit 743744edaa
2 changed files with 41 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -156,7 +156,7 @@ class ArmNode(Node):
# Grab velocities from message # Grab velocities from message
velocities = [ velocities = [
( (
msg.velocities[msg.joint_names.index(joint_name)] msg.velocities[msg.joint_names.index(joint_name)] # type: ignore
if joint_name in msg.joint_names if joint_name in msg.joint_names
else 0.0 else 0.0
) )

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

@@ -39,6 +39,7 @@ os.environ["SDL_AUDIODRIVER"] = (
CORE_STOP_MSG = CoreControl() # All zeros by default CORE_STOP_MSG = CoreControl() # All zeros by default
CORE_STOP_TWIST_MSG = Twist() # " CORE_STOP_TWIST_MSG = Twist() # "
ARM_STOP_MSG = ArmManual() # " ARM_STOP_MSG = ArmManual() # "
ARM_IK_STOP_MSG = TwistStamped() # "
BIO_STOP_MSG = BioControl() # " BIO_STOP_MSG = BioControl() # "
control_qos = qos.QoSProfile( control_qos = qos.QoSProfile(
@@ -148,25 +149,26 @@ class Headless(Node):
self.declare_parameter("use_bio", False) self.declare_parameter("use_bio", False)
self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
self.declare_parameter("arm_mode", "manual") self.declare_parameter("use_arm_ik", False)
self.arm_mode = ( self.use_arm_ik = (
self.get_parameter("arm_mode").get_parameter_value().string_value self.get_parameter("use_arm_ik").get_parameter_value().bool_value
) )
# NOTE: only applicable if use_old_topics == True # NOTE: only applicable if use_old_topics == True
self.declare_parameter("arm_manual_scheme", "old") self.declare_parameter("use_new_arm_manual_scheme", True)
self.arm_manual_scheme = ( self.use_new_arm_manual_scheme = (
self.get_parameter("arm_manual_scheme").get_parameter_value().string_value self.get_parameter("use_new_arm_manual_scheme")
.get_parameter_value()
.bool_value
) )
# Check parameter validity # Check parameter validity
if self.arm_mode not in ["manual", "ik"]: if self.use_arm_ik and self.use_old_topics:
self.get_logger().fatal("Old topics do not support arm IK control.")
sys.exit(1)
if not self.use_new_arm_manual_scheme and not self.use_old_topics:
self.get_logger().warn( self.get_logger().warn(
f"Invalid value '{self.arm_mode}' for arm_mode parameter. Defaulting to 'manual' (per-axis control)." f"New arm manual does not support old control scheme. Defaulting to new scheme."
)
if self.arm_manual_scheme not in ["old", "new"]:
self.get_logger().warn(
f"Invalid value '{self.arm_manual_scheme}' for arm_manual_scheme parameter. Defaulting to 'old' ('24 and '25 controls)."
) )
self.ctrl_mode = "core" # Start in core mode self.ctrl_mode = "core" # Start in core mode
@@ -205,6 +207,8 @@ class Headless(Node):
JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos
) )
# TODO: add new bio topics
################################################## ##################################################
# Timers # Timers
@@ -215,7 +219,7 @@ class Headless(Node):
# If using IK control, we have to "start" the servo node to enable it to accept commands # If using IK control, we have to "start" the servo node to enable it to accept commands
self.servo_start_client = None self.servo_start_client = None
if self.arm_mode == "ik": if self.use_arm_ik:
self.get_logger().info("Starting servo node for IK control...") self.get_logger().info("Starting servo node for IK control...")
self.servo_start_client = self.create_client( self.servo_start_client = self.create_client(
Trigger, "/servo_node/start_servo" Trigger, "/servo_node/start_servo"
@@ -242,7 +246,11 @@ class Headless(Node):
self.bio_publisher.publish(BIO_STOP_MSG) self.bio_publisher.publish(BIO_STOP_MSG)
else: else:
self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG) self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
self.arm_manual_pub_.publish(ARM_STOP_JOG_MSG) if self.use_arm_ik:
self.arm_ik_twist_publisher.publish(ARM_IK_STOP_MSG)
else:
self.arm_manual_pub_.publish(ARM_STOP_JOG_MSG)
# TODO: add bio here after implementing new topics
def send_controls(self): def send_controls(self):
"""Read the gamepad state and publish control messages""" """Read the gamepad state and publish control messages"""
@@ -389,11 +397,13 @@ class Headless(Node):
dpad_input = self.gamepad.get_hat(0) dpad_input = self.gamepad.get_hat(0)
# OLD MANUAL # OLD MANUAL
if self.arm_mode == "manual" and self.use_old_topics: # ==========
if not self.use_arm_ik and self.use_old_topics:
arm_input = ArmManual() arm_input = ArmManual()
# OLD ARM CONTROL SCHEME # OLD ARM MANUAL CONTROL SCHEME
if self.arm_manual_scheme == "old": if not self.use_new_arm_manual_scheme:
# EF Grippers # EF Grippers
if left_trigger > 0 and right_trigger > 0: if left_trigger > 0 and right_trigger > 0:
arm_input.gripper = 0 arm_input.gripper = 0
@@ -436,8 +446,8 @@ class Headless(Node):
if abs(right_stick_y) > 0.15: if abs(right_stick_y) > 0.15:
arm_input.axis3 = -1 * round(right_stick_y) arm_input.axis3 = -1 * round(right_stick_y)
# NEW ARM CONTROL SCHEME # NEW ARM MANUAL CONTROL SCHEME
if self.arm_manual_scheme == "new": if self.use_new_arm_manual_scheme:
# Right stick: EF yaw and axis 3 # Right stick: EF yaw and axis 3
# Left stick: axis 1 and 2 # Left stick: axis 1 and 2
# D-pad: axis 0 and _ # D-pad: axis 0 and _
@@ -490,7 +500,9 @@ class Headless(Node):
self.arm_publisher.publish(arm_input) self.arm_publisher.publish(arm_input)
# NEW MANUAL # NEW MANUAL
elif self.arm_mode == "manual" and not self.use_old_topics: # ==========
elif not self.use_arm_ik and not self.use_old_topics:
arm_input = JointJog() arm_input = JointJog()
arm_input.header.frame_id = "base_link" arm_input.header.frame_id = "base_link"
arm_input.header.stamp = self.get_clock().now().to_msg() arm_input.header.stamp = self.get_clock().now().to_msg()
@@ -557,8 +569,10 @@ class Headless(Node):
self.arm_manual_pub_.publish(arm_input) self.arm_manual_pub_.publish(arm_input)
# IK # IK (ONLY NEW)
elif self.arm_mode == "ik" and not self.use_old_topics: # =============
elif self.use_arm_ik:
arm_twist = TwistStamped() arm_twist = TwistStamped()
arm_twist.header.frame_id = "base_link" arm_twist.header.frame_id = "base_link"
arm_twist.header.stamp = self.get_clock().now().to_msg() arm_twist.header.stamp = self.get_clock().now().to_msg()
@@ -592,7 +606,7 @@ class Headless(Node):
# Triggers: EF Grippers # Triggers: EF Grippers
if left_trigger > 0 or right_trigger > 0: if left_trigger > 0 or right_trigger > 0:
arm_jointjog.joint_names.append("ef_gripper_left_joint") arm_jointjog.joint_names.append("ef_gripper_left_joint") # type: ignore
arm_jointjog.velocities.append(float(right_trigger - left_trigger)) arm_jointjog.velocities.append(float(right_trigger - left_trigger))
# Bumpers: angular x # Bumpers: angular x
@@ -636,6 +650,9 @@ class Headless(Node):
self.bio_publisher.publish(bio_input) self.bio_publisher.publish(bio_input)
else:
pass # TODO: implement new bio control topics
def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> float: def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> 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"""