From e3f750ec08209a0680c5a1e5abbb49d8f612b9f4 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Thu, 13 Feb 2020 15:15:40 +0000 Subject: [PATCH] Update --- object_tf_client.py | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/object_tf_client.py b/object_tf_client.py index 75447f3..13e1bcc 100755 --- a/object_tf_client.py +++ b/object_tf_client.py @@ -21,16 +21,19 @@ def tf_lookup(object_name): print('@@@@@@@@@@@@@@@@') print(rot) print('@@@@@@@@@@@@@@@@') - object_angles = tf.transformations.euler_from_quaternion([rot[2] , rot[0], rot[1], rot[2]], axes='sxyz') - xangle = object_angles[0]# + radians(360) - xangle1 = object_angles[1]# + radians(90) - xangle2 = object_angles[2]# + radians(-106) - print("Angles:") - print(xangle) - print(xangle1) - print(xangle2) - print() - target_quat = tf.transformations.quaternion_from_euler(xangle, xangle1, xangle2) + object_angles = tf.transformations.matrix_from_quaternion([rot[0] , rot[1], rot[2], rot[3]]) + print('@@@@@@@@@@@@@@@@') + print(object_angles) + print('@@@@@@@@@@@@@@@@') + # xangle = object_angles[0]# + radians(360) + # xangle1 = object_angles[1]# + radians(90) + # xangle2 = object_angles[2]# + radians(-106) + # print("Angles:") + # print(xangle) + # print(xangle1) + # print(xangle2) + # print() + target_quat = tf.transformations.quaternion_from_matrix(object_angles) target_pose = Pose() target_pose.position.x = trans[0] target_pose.position.y = trans[1]