From a9c3ffebe4afbf7ce953a425bc56c476383e0f3c Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Mon, 10 Feb 2020 22:32:05 +0000 Subject: [PATCH] Update --- object_tf_client.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/object_tf_client.py b/object_tf_client.py index bf64aad..d252242 100755 --- a/object_tf_client.py +++ b/object_tf_client.py @@ -3,6 +3,7 @@ import rospy import tf import time from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) +from math import radians def tf_lookup(object_name): do_try = True @@ -17,14 +18,18 @@ def tf_lookup(object_name): time.sleep(0.1) continue + object_angles = tf.transformations.euler_from_quaternion([rot[0] , rot[1] , rot[2], rot[3]]) + object_angles[0] += radians(90) + target_quat = tf.transformations.quaternion_from_euler(object_angles[0], object_angles[1], object_angles[2]) target_pose = Pose() target_pose.position.x = trans[0] target_pose.position.y = trans[1] target_pose.position.z = trans[2] - target_pose.orientation.x = rot[0] - target_pose.orientation.y = rot[1] - target_pose.orientation.z = rot[2] - target_pose.orientation.w = rot[3] + target_pose.orientation.x = target_quat[0] + target_pose.orientation.y = target_quat[1] + target_pose.orientation.z = target_quat[2] + target_pose.orientation.w = target_quat[3] + return target_pose if __name__ == "__main__":