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 tf
import time import time
from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
from math import radians
def tf_lookup(object_name): def tf_lookup(object_name):
do_try = True do_try = True
@ -17,14 +18,18 @@ def tf_lookup(object_name):
time.sleep(0.1) time.sleep(0.1)
continue 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 = Pose()
target_pose.position.x = trans[0] target_pose.position.x = trans[0]
target_pose.position.y = trans[1] target_pose.position.y = trans[1]
target_pose.position.z = trans[2] target_pose.position.z = trans[2]
target_pose.orientation.x = rot[0] target_pose.orientation.x = target_quat[0]
target_pose.orientation.y = rot[1] target_pose.orientation.y = target_quat[1]
target_pose.orientation.z = rot[2] target_pose.orientation.z = target_quat[2]
target_pose.orientation.w = rot[3] target_pose.orientation.w = target_quat[3]
return target_pose return target_pose
if __name__ == "__main__": if __name__ == "__main__":