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: (arm) add rev velocity feedback

Browse Source
This commit is contained in:
David
2025-12-06 18:58:32 -06:00
parent e644a3cad5
commit dbbbc28f95
1 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -33,7 +33,8 @@ thread = None
viccan_msg_len_dict = { viccan_msg_len_dict = {
53: 4, 53: 4,
54: 4, 54: 4,
55: 4 55: 4,
58: 4
} }
@@ -272,8 +273,8 @@ class SerialRelay(Node):
return return
def relay_fromvic(self, msg: VicCAN): def relay_fromvic(self, msg: VicCAN):
# Assume that the message is coming from Core if msg.mcu_name != "arm":
# skill diff if not return # ignore digit for now
# Check message len to prevent crashing on bad data # Check message len to prevent crashing on bad data
if msg.command_id in viccan_msg_len_dict: if msg.command_id in viccan_msg_len_dict:
@@ -284,7 +285,12 @@ class SerialRelay(Node):
) )
return return
# match msg.command_id: match msg.command_id:
case 58:
self.saved_joint_state.velocity[0] = math.radians(msg.data[0]) # Axis 0
self.saved_joint_state.velocity[1] = math.radians(msg.data[1]) # Axis 1
self.saved_joint_state.velocity[2] = math.radians(-msg.data[2]) # Axis 2 (inverted)
self.saved_joint_state.velocity[3] = math.radians(-msg.data[3]) # Axis 3 (inverted)
def publish_feedback(self): def publish_feedback(self):
self.socket_pub.publish(self.arm_feedback) self.socket_pub.publish(self.arm_feedback)