Rospy for tf and stretch_body

Ask
6

Got it, below is an example that moves the arm to 0.5m while using tf at the same time to output the end effector position. However, I don’t use robot.arm.move_to() in my example because it’s a Stretch Body method which will conflict with Stretch ROS. Instead, I use HelloNode.move_to_pose() which is part of Stretch ROS. If interested, I can explain why the difference matters.

Before running the following, perform a git update on Stretch ROS using: roscd stretch_core && git pull, and launch Stretch’s ROS driver using: roslaunch stretch_core stretch_driver.launch

import rospy, tf
import hello_helpers.hello_misc as hm

node = hm.HelloNode.quick_create('transformations')
listener = tf.TransformListener()
from_frame_rel = 'base_link'
to_frame_rel = 'link_grasp_center'

rospy.sleep(1.0)
rate = rospy.Rate(1)

node.move_to_pose({'joint_arm': 0.5}, return_before_done=True)

while not rospy.is_shutdown():
    try:
        translation, rotation = listener.lookupTransform(to_frame_rel, from_frame_rel, rospy.Time(0))
        rospy.loginfo('The pose of target frame %s with respect to %s is: \n %s, %s', to_frame_rel, from_frame_rel, translation, rotation)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        continue
    rate.sleep()

For more info on move_to_pose(), its API is available here.