Merge remote-tracking branch 'refs/remotes/origin/ur5' into ur5

arm_teleop/avp_teleop_node.py

-from geometry_msgs.msg import TransformStamped, PoseStamped
+from geometry_msgs.msg import PoseStamped
 
 import numpy as np
 from sensor_msgs.msg import Joy
 import time
 
-from std_msgs.msg import Float32MultiArray, Int32MultiArray, Float64MultiArray, String
+from std_msgs.msg import Float64MultiArray
 from .teleop_helper_functions import broadcast_tf, get_relative_quaternion
 from helper_functions.gamepad_functions import GamepadFunctions
 
@@ -41,6 +41,7 @@ class AVPTeleopNode(Node):
         # Publisher
         self.franka_pose_publisher = self.create_publisher(Float64MultiArray, '/franka/pose_demand', rclpy.qos.qos_profile_sensor_data)
         self.wrist_servo_demand_publisher = self.create_publisher(Float64MultiArray, '/wrist/servo_demand', rclpy.qos.qos_profile_sensor_data)
+        self.current_hand_transform_publisher = self.create_publisher(PoseStamped, '/hand_transform', 10)
 
         # Gamepad
         self.gp = GamepadFunctions()
@@ -215,7 +216,22 @@ class AVPTeleopNode(Node):
         # Computes the orientation by taking into account the wrist pose
         current_hand_quat = self.hand_base_to_wirst(hand_base)
         current_hand_trans = hand_base[:3]
-    
+
+        # Publish the pose of the hand (For recording a 6-dof pose of the hand)
+        msg = PoseStamped()
+        msg.header.frame_id = "hand"
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.pose.orientation.x = current_hand_quat[0]
+        msg.pose.orientation.y = current_hand_quat[1]
+        msg.pose.orientation.z = current_hand_quat[2]
+        msg.pose.orientation.w = current_hand_quat[3]
+
+        msg.pose.position.x = current_hand_trans[0]
+        msg.pose.position.y = current_hand_trans[1]
+        msg.pose.position.z = current_hand_trans[2]
+
+        self.current_hand_transform_publisher.publish(msg)
+
         diff = get_relative_quaternion(current_hand_quat, self.avp_wrist_q).as_euler("zyx", degrees = True)
 
         # Get current pitch yaw and see viability