[Melodic][TF Tutorial] 3. Writing a tf listener (Python)

Env.

VMware workstation(Ubuntu 18.04 amd64, ROS Melodic)

 

1. How to create a tf listener

$ roscd learning_tf
$ nano ./nodes/turtle_tf_listener.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

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

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

$ chmod +x ./nodes/turtle_tf_listener.py

 

1.1 tf.TransformListener()와 lookupTransform

은 아래와 같다.

https://github.com/ros/geometry/blob/melodic-devel/tf/src/tf/listener.py

GitHub - ros/geometry: Packages for common geometric calculations including the ROS transform library, "tf". Also includes ROS b

TransformListener는 TransformerROS를 상속하고 있고,

class TransformListener(TransformerROS):

    """
    TransformListener is a subclass of :class:`tf.TransformerROS` that
    subscribes to the ``"/tf"`` message topic, and calls :meth:`tf.Transformer.setTransform`
    with each incoming transformation message.
    In this way a TransformListener object automatically
    stays up to to date with all current transforms.  Typical usage might be::
        import tf
        from geometry_msgs.msg import PointStamped
        class MyNode:
            def __init__(self):
                self.tl = tf.TransformListener()
                rospy.Subscriber("/sometopic", PointStamped, self.some_message_handler)
                ...
            
            def some_message_handler(self, point_stamped):
                # want to work on the point in the "world" frame
                point_in_world = self.tl.transformPoint("world", point_stamped)
                ...
        
    """

TransformerROS는 Transformer를 상속하고 있으며,

class TransformerROS(Transformer):
    """
    TransformerROS extends the base class :class:`tf.Transformer`,
    adding methods for handling ROS messages. 
    """

Transformer와 lookupTransform은 아래와 같다.

class Transformer(object):

    def __init__(self, interpolate=True, cache_time=None):
        self._buffer = tf2_ros.Buffer(cache_time, debug=False)
        self._using_dedicated_thread = False
        
    def lookupTransform(self, target_frame, source_frame, time):
        msg = self._buffer.lookup_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time)
        t = msg.transform.translation
        r = msg.transform.rotation
        return [t.x, t.y, t.z], [r.x, r.y, r.z, r.w]

tf2_ros.Buffer에 대해서는 아래를 참고바란다.

https://github.com/ros/geometry2/blob/melodic-devel/tf2_ros/src/tf2_ros/buffer.py

GitHub - ros/geometry2: A set of ROS packages for keeping track of coordinate transforms.

 

2. Running the listener

$ nano ./launch/start_demo.launch

  <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" />
    </node>
    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
  </launch>

# 1st terminal
$ roslaunch learning_tf start_demo.launch

시작하면 가운데 있는 거북이(turtle1)로 다른 거북이(turtle2)가 이동을 한다.

 

또한, 화살표로 가운데의 거북이(turtle1)를 움직이면 다른 거북이(turtle2)가 따라온다.

 

3. Checking the results

3.1 rqt

# 2nd terminal
$ rqt

3.2 tf_echo

# 2nd terminal
$ rosrun tf tf_echo turtle1 turtle2
At time 1661263747.336
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.514, 0.858]
            in RPY (radian) [0.000, -0.000, 1.079]
            in RPY (degree) [0.000, -0.000, 61.831]
At time 1661263748.072
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.514, 0.858]
            in RPY (radian) [0.000, -0.000, 1.079]
            in RPY (degree) [0.000, -0.000, 61.831]
At time 1661263749.080
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.514, 0.858]
            in RPY (radian) [0.000, -0.000, 1.079]
            in RPY (degree) [0.000, -0.000, 61.831]
...

3.3 rviz

# 2nd terminal
$ rviz

 

ref.

http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29

tf/Tutorials/Writing a tf listener (Python) - ROS Wiki