Update
This commit is contained in:
parent
f0eaf45a40
commit
7ce4112146
28
object_tf_client.py
Normal file
28
object_tf_client.py
Normal 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
5
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user