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

fix(arm): populate missing feedback fields

Browse Source
This commit is contained in:
David
2026-03-26 02:40:58 -05:00
parent 7669ded344
commit cf699da0c6
1 changed files with 9 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -145,6 +145,10 @@ class ArmNode(Node):
# Combined Socket and Digit feedback # Combined Socket and Digit feedback
self.arm_feedback_new = ArmFeedback() self.arm_feedback_new = ArmFeedback()
self.arm_feedback_new.axis0_motor.id = 1
self.arm_feedback_new.axis1_motor.id = 2
self.arm_feedback_new.axis2_motor.id = 3
self.arm_feedback_new.axis3_motor.id = 4
# IK Arm pose # IK Arm pose
self.saved_joint_state = JointState() self.saved_joint_state = JointState()
@@ -287,6 +291,8 @@ class ArmNode(Node):
) )
return return
self.arm_feedback_new.header.stamp = msg.header.stamp
match msg.command_id: match msg.command_id:
case 53: # REV SPARK MAX feedback case 53: # REV SPARK MAX feedback
motorId = round(msg.data[0]) motorId = round(msg.data[0])
@@ -373,11 +379,14 @@ class ArmNode(Node):
) )
return return
self.arm_feedback_new.header.stamp = msg.header.stamp
match msg.command_id: match msg.command_id:
case 54: # Board voltages case 54: # Board voltages
self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0 self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0 self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0 self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
self.arm_feedback_new.digit_voltage.header.stamp = msg.header.stamp
case 55: # Arm joint positions case 55: # Arm joint positions
self.saved_joint_state.position[4] = math.radians( self.saved_joint_state.position[4] = math.radians(
msg.data[0] msg.data[0]