diff --git a/object_tf_client.py b/object_tf_client.py new file mode 100644 index 0000000..ea989ac --- /dev/null +++ b/object_tf_client.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python +import rospy +import tf +import time +from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) + +def tf_lookup(object_name): + do_try = True + listener = tf.TransformListener() + + while not rospy.is_shutdown() and do_try: + try: + (trans,rot) = listener.lookupTransform('c1', 'base', rospy.Time(0)) + do_try = False + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + print('Failed to get frame data, retrying...') + time.sleep(0.1) + continue + + 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] + return target_pose \ No newline at end of file diff --git a/run b/run index 2ab3248..7dc75fe 100755 --- a/run +++ b/run @@ -6,15 +6,16 @@ echo 2 mv main.py ../ echo 3 mv object_tf_service.py ../ +mv object_tf_client.py ../ echo 4 cd ../ echo 5 rm object_tf_service.pyc +rm object_tf_client.py echo 6 python main.py echo 7 mv main.py RoboticSandbox/ -echo 8 +mv object_tf_client.py RoboticSandbox/ mv object_tf_service.py RoboticSandbox/ -echo 9 echo Done