max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-10 22:32:05 +00:00
parent 0a4e266443
commit a9c3ffebe4
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
object_tf_client.py
View File

@ -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__":