TF101

Contents: Visulization of TF, Publishing, Broadcasting & Listening to TF, Static Transform, Joint and Robot State Publishers.

A transform specifies how data expressed in a frame can be transformed into a different frame. For instance, if you detect an obstacle with the laser at 3 cm in the front, this means that it is 3 cm from the laser, but not from the center of the robot (that is usually called the /base_link). To know the distance from the center of the robot, you need to transform the 3 cm from the /laser_frame to the /base_link frame (which is actually what the Path Planning system needs to know, what is the distance from the center of the robot to the obstacle).

First, we’ll define two frames (coordinate frames), one at the center of the laser and another one at the center of the robot. Usually, the center of the robot is placed at the rotational center of the robot. We’ll name the laser frame as base_laser and the robot frame as base_link.

Now, we need to define a relationship (in terms of position and orientation) between the base_laser and the base_link. For instance, we know that the base_laser frame is at a distance of 20 cm in the y axis and 10 cm in the x axis referring the base_link frame. Then we’ll have to provide this relationship to the robot. This relationship between the position of the laser and the base of the robot is known in ROS as the TRANSFORM between the laser and the robot.

Visulization of TF

View_frames

The view_frames ROS node generates a diagram with the current TF tree of the system.

rosrun tf view_frames

rqt_tf_tree

rqt_tf_tree gives the same functionality as the view_frames, with an interesting extra: you can refresh and see changes without having to generate another PDF file each time.

rosrun rqt_tf_tree rqt_tf_tree

Using echo /tf

There is a topic named /tf where all the TFs are published. The only problem is that ALL of them are published. In simple systems like this one, that isn’t a problem; but in massive systems, the quantity of data can be overwhelming. Therefore, the tf package gives a handy tool that filters which transform you are interested in and just shows you that one.

rostopic echo -n1 /tf

Using tf_echo

The /tf topic only publishes the direct TFs, not all the transforms between all the frames. tf_echo does return the transforms between any connected frames to you.

rosrun tf tf_echo [reference_frame] [target_frame]

TF Publisher, Broadcaster, Listener, Subscriber

TF Publisher

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

#!/usr/bin/env python  
import rospy
import tf2_ros
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped


rospy.init_node('fixed_tf2_broadcaster')

pub_tf = rospy.Publisher("/tf",TFMessage, queue_size=1)
message = TFMessage()

while not rospy.is_shutdown():
     # Run this loop at about 10Hz
    rospy.sleep(0.1)

    t1 = TransformStamped()
    t1.header.frame_id = "turtle1"
    t1.header.stamp = rospy.Time.now()
    t1.child_frame_id = "carrot1"
    t1.transform.translation.x = 0.0
    t1.transform.translation.y = 2.0
    t1.transform.translation.z = 0.0

    t1.transform.rotation.x = 0.0
    t1.transform.rotation.y = 0.0
    t1.transform.rotation.z = 0.0
    t1.transform.rotation.w = 1.0

    t2 = TransformStamped()
    t2.header.frame_id = "turtle1"
    t2.header.stamp = rospy.Time.now()
    t2.child_frame_id = "carrot2"
    t2.transform.translation.x = 0.0
    t2.transform.translation.y = 4.0
    t2.transform.translation.z = 0.0

    t2.transform.rotation.x = 0.0
    t2.transform.rotation.y = 0.0
    t2.transform.rotation.z = 0.0
    t2.transform.rotation.w = 1.0

    message = [t1,t2]
    pub_tf.publish(message)

rospy.spin()

TF Broadcaster Example - 1

1
2
3
4
5
6
7
8

def handle_turtle_pose(pose_msg, robot_name):
    br = tf2_ros.TransformBroadcaster()

    br.sendTransform((pose_msg.position.x,pose_msg.position.y,pose_msg.position.z),
                     (pose_msg.orientation.x,pose_msg.orientation.y,pose_msg.orientation.z,pose_msg.orientation.w),
                     rospy.Time.now(),
                     robot_name,
                     "/world")

You have to publish each element of the position and orientation inside a parenthesis, otherwise it might not work.There is also a very important element, which is the rospy.Time.now(). This is because TF really depends on time to make everything work and be able to play with past messages. Then, state the name of the child-frame you want to assign that model (robot_name) and the parent-frame, which, in this case, is /world.

TF Broadcaster Example - 2

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

#!/usr/bin/env python  
import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('my_moving_carrot_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(5.0)
    turning_speed_rate = 0.1
    while not rospy.is_shutdown():
        t = (rospy.Time.now().to_sec() * math.pi)*turning_speed_rate
        # Map to only one turn maximum [0,2*pi)
        rad_var = t % (2*math.pi)
        br.sendTransform((1.0 * math.sin(rad_var), 1.0 * math.cos(rad_var), 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "moving_carrot",
                         "turtle2")
        rate.sleep()

TF Broadcaster from Pose for turtlesim_node

NOTE: Requires setting of ROS params in launch file as shown below

1
2
3
4
5
6
7
8
9

<launch>
    <!--To get the Pose from turtle bot we have to launch turtlesim_node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node name="turtle1_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
        <param name="turtle" type="string" value="turtle1" />

    </node>
</launch>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import TransformStamped
from turtlesim.msg import Pose
import tf_conversions
import tf2_ros

def callback_function(pose):
    global turtlename
    br = tf2_ros.TransformBroadcaster()
    t = TransformStamped()

    t.header.stamp = rospy.Time.now()
    t.header.frame_id = 'world'
    t.child_frame_id = turtlename
    t.transform.translation.x = pose.x
    t.transform.translation.y = pose.y
    t.transform.translation.z = 0

    q = tf_conversions.transformations.quaternion_from_euler(0,0,pose.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

    br.sendTransform(t)


if __name__== '__main__':
    rospy.init_node('tf2_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose'% turtlename,Pose,callback_function)

    rospy.spin()

TF Listener

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

#!/usr/bin/env python  

import rospy
import tf
import math
import tf2_ros
from geometry_msgs.msg import Twist

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_listener')

    tfBuffer = tf2_ros.Buffer()
    tf2_ros.TransformListener(tfBuffer)

    turtle_name = "turtle2"

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())
            #print(trans)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rate.sleep()
            continue

        print(trans)
        rate.sleep()

NOTE: Using rospy.Time.now() in lookup_transform can cause errors. rospy.Time.now() > will ask for the frame with the current time, which might always not be avaiable immdeaditly. rospy.Time() > will ask for the latest frame that is available. To tackle this a time out argument can be given to the lookup_transform,

trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time.now(),rospy.Duration(1.0))

Time Travel

To get the tranform between two objects from the past the following can be used.

1
2
3
4

try:
    past = rospy.Time.now() - rospy.Duration(5.0)
    trans = tfBuffer.lookup_transform(turtle_name, 'carrot1', past, rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):

To get transforms between objects from different timelines the following can be used.

1
2
3
4
5

try:
    past = rospy.Time.now() - rospy.Duration(5.0)
    trans = tfBuffer.lookup_transform_full(target_frame=turtle_name,target_time=rospy.Time.now(), source_frame='carrot1',source_time=past,fixed_frame='world',timeout=rospy.Duration(1.0))

except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):

robot_state_publisher

Publishing all the TF’s manually is very tedious and hence ROS provides nice tool called robot state publisher to automate the task for you. In essence, it takes a file describing the morphology of the robot (aka URDF file) as input and it automatically publishes the TF for you.

1
2
3
4
5
6
7
8
9
10
11
12

<launch>

  <!-- Load the URDF file in the param server variable robot_description if it wasn't loaded before -->
  <param name="robot_description" command="cat $(find pi_robot_pkg)/urdf/pi_robot_v2.urdf"/>

  <!-- Publish TF with robot_state_publisher -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/pi_robot/joint_states" />
  </node>

</launch>

joint_state_publisher

There are two ways to publish non-fixed joint frames

Publish them directly

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

<launch>
  <param name="robot_description" command="cat $(find pi_robot_pkg)/urdf/pi_robot_v2.urdf" />

  <!-- send fake joint values -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="TRUE"/>
  </node>

  <!-- Combine joint values -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

  <!-- Show in RVIZ   -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find pi_robot_pkg)/launch/pi_robot.rviz"/>

</launch>

NOTE: This only works in RVIZ, meaning the robot in gazebo won’t be effected by the joint state values.

Publish them through Hardware/Simulation

Step 1: Define a joint that isn’t fixed in your URDF File

1
2
3
4
5
6
7
8

<joint name="left_shoulder_forward_joint" type="revolute">
    <parent link="left_shoulder_link"/>
    <child link="left_shoulder_forward_link"/>
    <origin xyz="0 0.025 0" rpy="0 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="3"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.7"/>
</joint>

Step 2: Define a Transmission for your joint in your URDF File

1
2
3
4
5
6
7
8
9
10

<transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_shoulder_forward_joint">
        <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
        <hardwareInterface>EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

Step 3: Define the new transmission controller with the name xxx_position_controller (left_shoulder_forward_joint_position_controller) in a configuration yaml file

1
2
3
4
5
6
7
8
9
10

pi_robot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  left_shoulder_forward_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: left_shoulder_forward_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}

Step 4: Start everything through a launch file.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

<launch>

  <!-- Load the URDF file in the param server variable robot_description if it wasn't loaded before -->
  <param name="robot_description" command="cat $(find pi_robot_pkg)/urdf/pi_robot_v2.urdf" />
<!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find pi_robot_pkg)/config/pirobot_control.yaml" command="load"/>

  <node name="controller_spawner"
        pkg="controller_manager"
        type="spawner"
        respawn="false"
        output="screen"
        ns="/pi_robot"
        args="head_pan_joint_position_controller
              head_tilt_joint_position_controller
              torso_joint_position_controller
              left_shoulder_forward_joint_position_controller
              right_shoulder_forward_joint_position_controller
              left_shoulder_up_joint_position_controller
              right_shoulder_up_joint_position_controller
              left_elbow_joint_position_controller
              right_elbow_joint_position_controller
              left_wrist_joint_position_controller
              right_wrist_joint_position_controller
              joint_state_controller"/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/pi_robot/joint_states" />
  </node> 

<!-- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>-->


</launch>

Static Transforms

There are some cases where changing the URDF is not advisable or it simply makes no sense to add it to the robot model.urdf. These are the cases where publishing a static transform, especially through the launch file or commands, is more convenient.

Through Command Line

The following syntax can be used to publish a static transform via command line. rosrun tf static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

Through Launch files

The following syntax can be used to publish a static transform via launch files.

1
2
3
4
5

<launch>
    <node pkg="tf" type="static_transform_publisher" name="name_of_node" 
          args="x y z yaw pitch roll frame_id child_frame_id period_in_ms">
    </node>
</launch>

Extra Content

Spawn a URDF Model in gazebo

This is an example of a launch file you could use to spawn URDF models into a Gazebo world:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

<?xml version="1.0" encoding="UTF-8"?>

<launch>

    <arg name="x" default="0.0" />
    <arg name="y" default="0.0" />
    <arg name="z" default="0.0" />

    <arg name="urdf_robot_file" default="$(find your_pkg)/urdf/your_robot.urdf" />
    <arg name="robot_name" default="your_robot_model_name" />


    <!-- Load the URDF into the ROS Parameter Server -->
    <param name="robot_description" command="cat $(arg urdf_robot_file)" />

    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -x $(arg x) -y $(arg y) -z $(arg z)  -model $(arg robot_name) -param robot_description"/>
</launch>

Delete a model from gazebo

To get a list of all the current models in gazebo:

osservice call /gazebo/get_world_properties "{}"

To delete a paticular model from the above list:

rosservice call /gazebo/delete_model "model_name: 'your_robot_model_name'"