Example 10
This tutorial provides you an idea of what tf2 can do in the Python track. We will elaborate how to create a tf2 static broadcaster and listener.
tf2 Static Broadcaster
For the tf2 static broadcaster node, we will be publishing three child static frames in reference to the link_mast, link_lift, and link_wrist_yaw frames.
Begin by starting up the stretch driver launch file.
# Terminal 1
roslaunch stretch_core stretch_driver.launch
# Terminal 2
rosrun rviz rviz -d /home/hello-robot/catkin_ws/src/stretch_tutorials/rviz/tf2_broadcaster_example.rviz
# Terminal 3
cd catkin_ws/src/stretch_tutorials/src/
python tf2_broadcaster.py
The gif below visualizes what happens when running the previous node.
OPTIONAL: If you would like to see how the static frames update while the robot is in motion, run the stow_command_node.py and observe the tf frames in RViz.
# Terminal 4
cd catkin_ws/src/stretch_tutorials/src/
python stow_command.py
The Code
#!/usr/bin/env python
import rospy
import tf.transformations
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
class FixedFrameBroadcaster():
"""
This node publishes three child static frames in reference to their parent frames as below:
parent -> link_mast child -> fk_link_mast
parent -> link_lift child -> fk_link_lift
parent -> link_wrist_yaw child -> fk_link_wrist_yaw
"""
def __init__(self):
"""
A function that creates a broadcast node and publishes three new transform
frames.
:param self: The self reference.
"""
self.br = StaticTransformBroadcaster()
self.mast = TransformStamped()
self.mast.header.stamp = rospy.Time.now()
self.mast.header.frame_id = 'link_mast'
self.mast.child_frame_id = 'fk_link_mast'
self.mast.transform.translation.x = 0.0
self.mast.transform.translation.y = 2.0
self.mast.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(1.5707, 0, -1.5707)
self.mast.transform.rotation.x = q[0]
self.mast.transform.rotation.y = q[1]
self.mast.transform.rotation.z = q[2]
self.mast.transform.rotation.w = q[3]
self.lift = TransformStamped()
self.lift.header.stamp = rospy.Time.now()
self.lift.header.frame_id = 'link_lift'
self.lift.child_frame_id = 'fk_link_lift'
self.lift.transform.translation.x = 0.0
self.lift.transform.translation.y = 1.0
self.lift.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(1.5707, 0, -1.5707)
self.lift.transform.rotation.x = q[0]
self.lift.transform.rotation.y = q[1]
self.lift.transform.rotation.z = q[2]
self.lift.transform.rotation.w = q[3]
self.wrist = TransformStamped()
self.wrist.header.stamp = rospy.Time.now()
self.wrist.header.frame_id = 'link_wrist_yaw'
self.wrist.child_frame_id = 'fk_link_wrist_yaw'
self.wrist.transform.translation.x = 0.0
self.wrist.transform.translation.y = 1.0
self.wrist.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(1.5707, 0, -1.5707)
self.wrist.transform.rotation.x = q[0]
self.wrist.transform.rotation.y = q[1]
self.wrist.transform.rotation.z = q[2]
self.wrist.transform.rotation.w = q[3]
self.br.sendTransform([self.mast, self.lift, self.wrist])
rospy.loginfo('Publishing TF frames. Use RViz to visualize')
if __name__ == '__main__':
rospy.init_node('tf2_broadcaster')
FixedFrameBroadcaster()
rospy.spin()
The Code Explained
Now let's break the code down.
#!/usr/bin/env python
import rospy
import tf.transformations
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
tf.transformations
to get quaternion values from Euler angles. Import the TransformStamped
from the geometry_msgs.msg
package because we will be publishing static frames and it requires this message type. The tf2_ros
package provides an implementation of a tf2_ros.StaticTransformBroadcaster
to help make the task of publishing transforms easier.
def __init__(self):
"""
A function that creates a broadcast node and publishes three new transform
frames.
:param self: The self reference.
"""
self.br = StaticTransformBroadcaster()
TransformStamped
object which will be the message we will send over once populated.
self.mast = TransformStamped()
self.mast.header.stamp = rospy.Time.now()
self.mast.header.frame_id = 'link_mast'
self.mast.child_frame_id = 'fk_link_mast'
rospy.Time.now()
. Then, we need to set the name of the parent frame of the link we're creating, in this case link_mast. Finally, we need to set the name of the child frame of the link we're creating. In this instance, the child frame is fk_link_mast.
self.mast.transform.translation.x = 0.0
self.mast.transform.translation.y = 2.0
self.mast.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(1.5707, 0, -1.5707)
self.wrist.transform.rotation.x = q[0]
self.wrist.transform.rotation.y = q[1]
self.wrist.transform.rotation.z = q[2]
self.wrist.transform.rotation.w = q[3]
quaternion_from_euler()
function takes in a Euler angle argument and returns a quaternion values. Then set the rotation values to the transformed quaternions.
This process will be completed for the link_lift and link_wrist_yaw as well.
self.br.sendTransform([self.mast, self.lift, self.wrist])
sendTransform()
function.
rospy.init_node('tf2_broadcaster')
FixedFrameBroadcaster()
rospy.init_node(NAME, ...)
, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate the FixedFrameBroadcaster()
class.
rospy.spin()
tf2 Static Listener
In the previous section of the tutorial, we created a tf2 broadcaster to publish three static transform frames. In this section we will create a tf2 listener that will find the transform between fk_link_lift and link_grasp_center.
Begin by starting up the stretch driver launch file.
# Terminal 1
roslaunch stretch_core stretch_driver.launch
# Terminal 2
cd catkin_ws/src/stretch_tutorials/src/
python tf2_broadcaster.py
# Terminal 3
cd catkin_ws/src/stretch_tutorials/src/
python tf2_listener.py
[INFO] [1659551318.098168]: The pose of target frame link_grasp_center with reference from fk_link_lift is:
translation:
x: 1.08415191335
y: -0.176147838153
z: 0.576720021135
rotation:
x: -0.479004489528
y: -0.508053545368
z: -0.502884087254
w: 0.509454501243
The Code
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import TransformStamped
import tf2_ros
class FrameListener():
"""
This Class prints the transformation between the fk_link_mast frame and the
target frame, link_grasp_center.
"""
def __init__(self):
"""
A function that initializes the variables and looks up a transformation
between a target and source frame.
:param self: The self reference.
"""
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
from_frame_rel = 'link_grasp_center'
to_frame_rel = 'fk_link_lift'
rospy.sleep(1.0)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
try:
trans = tf_buffer.lookup_transform(to_frame_rel,
from_frame_rel,
rospy.Time())
rospy.loginfo('The pose of target frame %s with reference from %s is: \n %s', from_frame_rel, to_frame_rel, trans.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logwarn(' Could not transform %s from %s ', to_frame_rel, from_frame_rel)
rate.sleep()
if __name__ == '__main__':
rospy.init_node('tf2_listener')
FrameListener()
rospy.spin()
The Code Explained
Now let's break the code down.
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import TransformStamped
import tf2_ros
You need to import rospy if you are writing a ROS Node. Import the TransformStamped
from the geometry_msgs.msg
package because we will be publishing static frames and it requires this message type. The tf2_ros
package provides an implementation of a tf2_ros.TransformListener
to help make the task of receiving transforms easier.
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
TransformListener
object. Once the listener is created, it starts receiving tf2 transformations over the wire, and buffers them for up to 10 seconds.
from_frame_rel = 'link_grasp_center'
to_frame_rel = 'fk_link_lift'
rospy.sleep(1.0)
rate = rospy.Rate(1)
try:
trans = tf_buffer.lookup_transform(to_frame_rel,
from_frame_rel,
rospy.Time())
rospy.loginfo('The pose of target frame %s with reference from %s is: \n %s', from_frame_rel, to_frame_rel, trans.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logwarn(' Could not transform %s from %s ', to_frame_rel, from_frame_rel)
lookup_transform()
function.
rospy.init_node('tf2_listener')
FrameListener()
rospy.init_node(NAME, ...)
, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate the FrameListener()
class.
rospy.spin()