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 ../
|
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
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user