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

debug printing

Browse Source
This commit is contained in:
Tristan McGinnis
2025-04-26 12:02:30 -05:00
committed by David
parent dde9d61a33
commit 5c41d66404
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -396,11 +396,15 @@ class SerialRelay(Node):
def send_ik(self, msg):
input_raw = msg.movement_vector # [x, y, z]
#input_x = input_raw[0]
#input_y = input_raw[1]
#input_z = input_raw[2]
# Debug output
msg = String()
msg.data = "From IK Control Got Vector: " + str(input_raw)
self.debug_pub.publish(msg)
tempMsg = String()
tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
#self.get_logger().info(f"[IK Control] {tempMsg.data}")
self.debug_pub.publish(tempMsg)
# normalize the vector
input_norm = np.linalg.norm(input_raw) / 2.0
@@ -410,8 +414,8 @@ class SerialRelay(Node):
#Target position is current position + normalized vector
target_position = self.arm.get_position() + input_norm
msg.data = "Target Position: " + str(target_position)
self.debug_pub.publish(msg)
tempMsg.data = "Target Position: " + str(target_position)
self.debug_pub.publish(tempMsg)
if(self.arm.perform_ik(self.arm.get_position()+input_norm)):
#send command to control