Introduction to ROS application development programming of tf coordinate system broadcasting and monitoring (python)

ROS is the abbreviation of Robot Operating System. This article introduces the introduction of ROS application development, programming of TF coordinate system broadcasting and monitoring. There are 2 turtles in the little turtle simulation. The keyboard controls the first turtle to move around, and both turtles broadcast their coordinates. The monitor listens to the coordinates of the two turtles, obtains the relative position, and controls the second turtle to keep following the first turtle. The code is Python. The test verification adopts the roslaunch method.

Project package creation

This article is a continuation of the article Programming of tf coordinate system broadcasting and monitoring in the introduction to ROS application development . If you have done that experiment, the project package has been established, so skip this section.

The  ROS workspace  is established in the article ROS Development and Application Preparation: Create a Workspace . Now, create a functional package:

cd ~/catkin_ws/src

catkin_create_pkg learning_tf roscpp rospy tf turtlesim

The first line is to return to the src directory of the workspace. The function package creation must be run in this directory.

The second line is the command to create the function package, the first parameter is the name of the function package, here is learning_tf, and the next is the dependency library of the function package, here is a total of 4 dependency libraries of roscpp rospy tf turtlesim.

After the above command is executed, you can see a directory named learning_tf in the src directory with the following content:

CMakeLists.txt  include  package.xml   src

Broadcaster code

In the project directory, which is also the ~/catkin_ws/src/learning_tf/ directory, create a new folder scripts, and create a file turtle_tf_broadcaster.py,

cd ~//catkin_ws/src/learning_tf/

mkdir scripts

cd scripts

nano turtle_tf_broadcaster.py

The content is:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

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()

First define a callback function, which includes the establishment of a broadcast

The main program, first define a subscriber node, set the callback function
Once the data is received, call the callback function

More detailed content visit: http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28Python%29

Listener code

In the scripts directory of the project directory, which is also the ~/catkin_ws/src/learning_tf/scripts directory, create a file turtle_tf_listener.py,

cd ~//catkin_ws/src/learning_tf/scripts

nano turtle_tf_listener.py

The content is:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

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()

The code is introduced at  http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29 

launch file to start the test

Python code does not need to be compiled, but should be set as an executable file

 chmod +x *.py

Here we use roslaunch to start the test:

 cd ~//catkin_ws/src/learning_tf/

mkdir launch

cd launch

nano start_tf_demo_py.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">
          <param name="turtle" type="string" value="turtle1" />
        </node>
        <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
          <param name="turtle" type="string" value="turtle2" />
        </node>

    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />

</launch>

Save and exit

Start roslaunch with the following command

roslaunch  start_tf_demo_py.launch

This command is in the folder where launch is located, otherwise the path name is required in front of it.

You can also start with a project package:

roslaunch learning_tf  start_tf_demo_py.launch

See the simulated turtle interface, go back to the startup terminal and use the arrow keys to control the movement of the first turtle, you can see the second turtle following.

The effect is as follows:

That's it for the introduction.