tf2 Tutorial

Intro

• Resources

  • https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html
  • The Transform System (tf2)
  • tf2

• Install demo package

sudo apt-get install ros-humble-rviz2 ros-humble-turtle-tf2-py ros-humble-tf2-ros ros-humble-tf2-tools ros-humble-turtlesim

• Run demo to start turtlesim with 2 turtles
  • The tf2 library was used to create 3 coordinate frames
    • world frame
    • turtle1 frame
    • turtle2 frame
  • A tf2 broadcaster publishes the turtle coordinate frames
  • A tf2 listener computes the difference in turtle frames to follow the main turtle

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

• Run the following command in another terminal

ros2 run turtlesim turtle_teleop_key

tf2 tools

• See The Transform System (tf2)
• Use view_frames to create a diagram of the frames broadcast by tf2 over ROS

ros2 run tf2_tools view_frames

[INFO] [1736039682.543490745] [view_frames]: Listening to tf data for 5.0 seconds...
[INFO] [1736039687.555903700] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1736039687.558138009] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="turtle1: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.706\n  most_recent_transform: 1736039687.554910\n  oldest_transform: 1736039682.531490\n  buffer_length: 5.023\nturtle2: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.705\n  most_recent_transform: 1736039687.554977\n  oldest_transform: 1736039682.531442\n  buffer_length: 5.024\n")

• Use tf2_echo to debug or analyze transformations in a ROS system by displaying the relative pose (position and orientation) between two coordinate frames

ros2 run tf2_ros tf2_echo [source_frame] [target_frame]

• Show the transfrom of turtle2 frame with respect to turtle1 frame

ros2 run tf2_ros tf2_echo turtle2 turtle1

• Use rviz2, a visualization tool, to examine the tf2 frames

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

In the context of the ROS tf (transform) framework, the transform data provides the relationship between the two frames: the turtle1 frame (reference) and the turtle2 frame (follower). This includes:

Components of the Transform

1. Translation (Position Difference):

   • The difference in position between turtle1 and turtle2, usually expressed as a 2D vector ((dx), (dy)) in the turtle1 frame. This tells you where turtle2 is relative to turtle1’s position.
   $$dx=x_{\text{turtle1}}-x_{\text{turtle2}}$$ $$dy=y_{\text{turtle1}}-y_{\text{turtle2}}$$

2. Rotation (Orientation Difference):

   • The difference in orientation between turtle1 and turtle2, often expressed as a single angular value ((yaw)) in 2D (since turtlesim operates in a plane). This tells you how much turtle2 needs to rotate to face the direction of turtle1.
   $$yaw={\theta }_{\text{turtle1}}-{\theta }_{\text{turtle2}}$$

Transform in Action

1. The listener node continuously receives the transformation data (translation and rotation) between turtle1 and turtle2.

2. The control logic uses this transform to:

   • Compute the angular difference (needed to rotate turtle2).
   • Compute the linear distance (needed to move turtle2 forward).

3. These calculated differences are used to set the velocities for turtle2:

   • Angular Velocity aligns turtle2 with turtle1.
   • Linear Velocity moves turtle2 toward turtle1.

Example

1. Given Information:

   • turtle1 is at the origin: ((0, 0)).
   • turtle2 starts at ((1.544, 3.544)) and faces 0 degrees (along the positive x-axis).

2. Relative Translation:

   • The position vector from turtle2 to turtle1 is:
   $$dx=0-1.544=-1.544$$ $$dy=0-3.544=-3.544$$

3. Distance Between Turtles:

   • The Euclidean distance between the two turtles is:
   $$\text{distance}=\sqrt{d{x}^2+d{y}^2}$$

   Substituting the values:

   $$\text{distance}=\sqrt{(-1.544{)}^2+(-3.544{)}^2}=3.847\text{units}$$

4. Angle to Face Turtle1:

   • The angle turtle2 needs to rotate to face turtle1 is given by:
   $$\theta =\arctan 2(dy,dx)$$

   Substituting:

   $$\theta =\arctan 2(-3.544,-1.544)$$

   This evaluates to:

   $$\theta \approx -1.977\text{radians}(\text{or }-113.3^{\circ })$$

5. Action Plan:

   • Angular Velocity: Rotate turtle2 by ( \theta ) until it aligns with the vector pointing to turtle1.
   • Linear Velocity: Move turtle2 forward along this vector to close the distance.

---

Sample Code with Motion Strategy

The ROS implementation will first align turtle2 with the direction of turtle1 and then move it forward:

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
 
def calculate_angle_and_distance(turtle1_pose, turtle2_pose):
    # Relative position
    dx = turtle1_pose.x - turtle2_pose.x
    dy = turtle1_pose.y - turtle2_pose.y
 
    # Distance between turtles
    distance = math.sqrt(dx**2 + dy**2)
 
    # Angle that turtle2 needs to face
    desired_angle = math.atan2(dy, dx)
 
    return desired_angle, distance
 
def move_turtle2():
    rospy.init_node('turtle2_follow', anonymous=True)
 
    # Publishers and subscribers
    pub = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
    turtle2_pose = rospy.wait_for_message('/turtle2/pose', Pose)
    turtle1_pose = rospy.wait_for_message('/turtle1/pose', Pose)
 
    rate = rospy.Rate(10)  # 10 Hz
 
    while not rospy.is_shutdown():
        # Calculate required angle and distance
        desired_angle, distance = calculate_angle_and_distance(turtle1_pose, turtle2_pose)
 
        # Angle difference (rotation needed for turtle2)
        angle_difference = desired_angle - turtle2_pose.theta
 
        # Normalize the angle difference to [-pi, pi]
        angle_difference = math.atan2(math.sin(angle_difference), math.cos(angle_difference))
 
        # Velocity commands
        twist = Twist()
 
        # Rotate to align with turtle1
        if abs(angle_difference) > 0.01:  # Allow small tolerance
            twist.angular.z = 2.0 * angle_difference  # Proportional control
            twist.linear.x = 0.0  # No forward motion until aligned
        else:
            # Move forward when aligned
            twist.angular.z = 0.0
            twist.linear.x = 1.5 * distance  # Proportional control
 
        # Publish velocity
        pub.publish(twist)
 
        # Update current pose
        turtle2_pose = rospy.wait_for_message('/turtle2/pose', Pose)
 
        rate.sleep()
 
if __name__ == '__main__':
    try:
        move_turtle2()
    except rospy.ROSInterruptException:
        pass

---

Expected Behavior

1. turtle2 computes the angle difference (\theta) and rotates to face turtle1.
2. Once aligned, it moves forward along the computed direction vector, closing the distance between the two turtles.

This ensures turtle2 correctly rotates to face turtle1 and moves toward it, respecting the given scenario.