This commit is contained in:
Max Hunt 2020-02-10 20:48:50 +00:00
parent f0eaf45a40
commit 7ce4112146
2 changed files with 31 additions and 2 deletions

28
object_tf_client.py Normal file
View File

@ -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

5
run
View File

@ -6,15 +6,16 @@ echo 2
mv main.py ../ mv main.py ../
echo 3 echo 3
mv object_tf_service.py ../ mv object_tf_service.py ../
mv object_tf_client.py ../
echo 4 echo 4
cd ../ cd ../
echo 5 echo 5
rm object_tf_service.pyc rm object_tf_service.pyc
rm object_tf_client.py
echo 6 echo 6
python main.py python main.py
echo 7 echo 7
mv main.py RoboticSandbox/ mv main.py RoboticSandbox/
echo 8 mv object_tf_client.py RoboticSandbox/
mv object_tf_service.py RoboticSandbox/ mv object_tf_service.py RoboticSandbox/
echo 9
echo Done echo Done