[Melodic][TF Tutorial] 2. Writing a tf broadcaster (Python)

Env.

VMware workstation(Ubuntu 18.04 amd64, ROS Melodic)

 

0. Create package

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
$ cd ..
$ catkin_make
$ source ./devel/setup.bash

 

1. How to broadcast transforms

$ roscd learning_tf
$ mkdir nodes
$ nano ./nodes/turtle_tf_broadcaster.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

$ chmod +x ./nodes/turtle_tf_broadcaster.py

 

1.1 tf.TransformBroadcaster()와 sendTransform

에 대해서는 아래와 같다.

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

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

class TransformBroadcaster:
    """
    :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic.
    """

    def __init__(self, queue_size=100):
        self.tf2_broadcaster = tf2_ros.transform_broadcaster.TransformBroadcaster()

    def sendTransform(self, translation, rotation, time, child, parent):
        """
        :param translation: the translation of the transformtion as a tuple (x, y, z)
        :param rotation: the rotation of the transformation as a tuple (x, y, z, w)
        :param time: the time of the transformation, as a rospy.Time()
        :param child: child frame in tf, string
        :param parent: parent frame in tf, string
        Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
        """

        t = geometry_msgs.msg.TransformStamped()
        t.header.frame_id = parent
        t.header.stamp = time
        t.child_frame_id = child
        t.transform.translation.x = translation[0]
        t.transform.translation.y = translation[1]
        t.transform.translation.z = translation[2]

        t.transform.rotation.x = rotation[0]
        t.transform.rotation.y = rotation[1]
        t.transform.rotation.z = rotation[2]
        t.transform.rotation.w = rotation[3]

        self.sendTransformMessage(t)

 

2. Running the broadcaster

$ mkdir launch
$ 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>

  </launch>

# 1st terminal
$ roslaunch learning_tf start_demo.launch

 

3. Checking the results

3.1 tf_echo

# 2nd terminal
$ rosrun tf tf_echo /world /turtle1
At time 1661260426.290
- Translation: [5.113, 7.960, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.953, 0.302]
            in RPY (radian) [0.000, -0.000, 2.528]
            in RPY (degree) [0.000, -0.000, 144.844]
At time 1661260427.060
- Translation: [5.113, 7.960, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.953, 0.302]
            in RPY (radian) [0.000, -0.000, 2.528]
            in RPY (degree) [0.000, -0.000, 144.844]
At time 1661260428.050
- Translation: [5.113, 7.960, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.953, 0.302]
            in RPY (radian) [0.000, -0.000, 2.528]
            in RPY (degree) [0.000, -0.000, 144.844]
At time 1661260429.059
- Translation: [5.113, 7.960, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.953, 0.302]
            in RPY (radian) [0.000, -0.000, 2.528]
            in RPY (degree) [0.000, -0.000, 144.844]
...

3.2 rqt

# 2nd terminal
$ rqt

3.3 rviz

# 2nd terminal
$ rviz

 

ref.

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

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