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

Added debug output to /arm/feedback/debug for testing IK

Browse Source
This commit is contained in:
Tristan McGinnis
2025-04-18 13:34:53 -05:00
committed by David
parent 4420e83981
commit 3985c11ae2
1 changed files with 21 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -136,7 +136,7 @@ class SerialRelay(Node):
self.recordMotorFeedback(output) self.recordMotorFeedback(output)
self.get_logger().info(f"[MCU] {output}") self.get_logger().info(f"[MCU] {output}")
msg = String() msg = String()
msg.data = output msg.data = "From MCU Got: " + output
self.debug_pub.publish(msg) self.debug_pub.publish(msg)
except serial.SerialException: except serial.SerialException:
self.get_logger().info("SerialException caught... closing serial port.") self.get_logger().info("SerialException caught... closing serial port.")
@@ -397,19 +397,39 @@ class SerialRelay(Node):
def send_ik(self, msg): def send_ik(self, msg):
input_raw = msg.movement_vector # [x, y, z] input_raw = msg.movement_vector # [x, y, z]
# Debug output
msg = String()
msg.data = "From IK Control Got Vector: " + str(input_raw)
self.debug_pub.publish(msg)
# normalize the vector # normalize the vector
input_norm = np.linalg.norm(input_raw) / 2.0 input_norm = np.linalg.norm(input_raw) / 2.0
#Target position is current position + normalized vector #Target position is current position + normalized vector
target_position = self.arm.get_position() + input_norm target_position = self.arm.get_position() + input_norm
msg.data = "Target Position: " + str(target_position)
self.debug_pub.publish(msg)
if(self.arm.perform_ik(self.arm.get_position()+input_norm)): if(self.arm.perform_ik(self.arm.get_position()+input_norm)):
#send command to control #send command to control
command = "can_relay_tovic,arm,32," + str(self.arm.ik_angles[0]) + "," + str(self.arm.ik_angles[1]) + "," + str(self.arm.ik_angles[2]) + "," + str(self.arm.ik_angles[3]) + "\n" command = "can_relay_tovic,arm,32," + str(self.arm.ik_angles[0]) + "," + str(self.arm.ik_angles[1]) + "," + str(self.arm.ik_angles[2]) + "," + str(self.arm.ik_angles[3]) + "\n"
self.send_cmd(command) self.send_cmd(command)
self.get_logger().info(f"IK Success: {target_position}") self.get_logger().info(f"IK Success: {target_position}")
msg = String()
msg.data = "IK Success: " + str(target_position)
self.debug_pub.publish(msg)
msg.data = "Sending: " + str(command)
self.debug_pub.publish(msg)
else: else:
self.get_logger().info("IK Fail") self.get_logger().info("IK Fail")
msg = String()
msg.data = "IK Fail"
self.debug_pub.publish(msg)
# Manual control for Wrist/Effector # Manual control for Wrist/Effector