ROS exploration Summary (12)-Coordinate System

In robot control, the coordinate system is very important. In Ros, the TF software library is used for coordinate transformation.

Link: http://www.ros.org/wiki/tf/Tutorials#Learning_tf

1. TF introduction we will introduce the functions of TF through a small instance. 1. Install the turtle package

$ rosdep install turtle_tf rviz$ rosmake turtle_tf rviz

2. Run the demo

Run a simple demo:

$ roslaunch turtle_tf turtle_tf_demo.launch

Then we can see two turtles.

This routine carries the turtlesim simulation, which can be controlled by the keyboard when the terminal is activated.

You can find that the second turtle will follow the turtle you moved.

3. Demo analysis next let's take a look at what ROs has done.
This routine uses TF to create three coordinate frames: A World frame, a turtle1 frame, and a turtle2 frame. Then, TF broadcaster is used to publish the coordinate frame of the turtle, and TF listener is used to calculate the difference between the coordinate frames of the turtle, so that the second turtle follows the first turtle.
We can use the TF tool for specific research.

$ rosrun tf view_frames

Then we will see a few prompts and generate a frames.pdf file.

This file describes the relationships between coordinate systems. The three nodes are three coordinate frames, And/world is the parent coordinate frame of the other two turtle coordinate frames. It also contains the sending frequency and recent time required for debugging.
Tf also provides a tf_echo tool to view the relationship between two Broadcast frames. Let's take a look at how the coordinates of the second turtle come from the first turtle.

$ rosrun tf tf_echo turtle1 turtle2

Control a tortoise. The Coordinate Conversion Relationship of the second tortoise is displayed in the terminal.

We can also see the relationship between the three through the graphic interface of rviz.

$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.vcg

Move the tortoise and you can see that the coordinates in rviz will follow the changes. The lower-left corner is/world, and the other two are the coordinate frames of turtles.
Next we will analyze this instance in detail. Ii. Writing a tf broadcaster1. Create a package

$ roscd tutorials$ roscreate-pkg learning_tf tf roscpp rospy turtlesim$ rosmake learning_tf

2. Broadcast transforms

First, let's take a look at how to publish a coordinate frame to tf.
Code File:/nodes/turtle_tf_broadcaster.py

#! /Usr/bin/env python import roslibroslib. load_manifest ('learning _ tf') import rospyimport tfimport turtlesim. msgdef 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") # Release the turtle translation and flip if _ name _ = '_ main _': rospy. init_node ('turtle _ tf_broadcaster ') turtlen Ame = rospy. get_param ('~ Turtle') # Get the turtle name (turtle1, turtle2) rospy. subscriber ('/% s/pose' % turtlename, turtlesim. msg. pose, handle_turtle_pose, turtlename) # subscribe to topic "turtleX/pose" rospy. spin ()

Create the launch file 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>

Run:

$ roslaunch learning_tf start_demo.launch

You can see that only the turtles are transplanted. Open the tf_echo information window:

$ rosrun tf tf_echo /world /turtle1 

The origin of the world coordinate system is at the very least. The Conversion Relationship Between turtle1 is actually the Coordinate Position and Rotation Angle of turtle1 in the world coordinate system.

Iii. Writing a TF listener

In this step, we will see how to use tf for Frame Conversion. First, write a tf listener (nodes/turtle_tf_listener.py ):

#! /Usr/bin/env python import roslibroslib. load_manifest ('learning _ tf') import rospyimport mathimport tfimport turtlesim. msgimport turtlesim. srvif _ name _ = '_ main _': rospy. init_node ('tf _ turtle ') listener = tf. transformListener () # After TransformListener is created, it will start to accept tf broadcast information. Up to 10 s of rospy can be cached. wait_for_service ('spawn ') spawner = rospy. serviceProxy ('spawn ', turtlesim. srv. spawn) spawner (4, 2, 0, 'turtle2') turtle_vel = rospy. publisher ('turtle2/command_velocity ', turtlesim. msg. velocity) rate = rospy. rate (10.0) while not rospy. is_shutdown (): try: (trans, rot) = listener. lookupTransform ('/turtle2', '/turtle1', rospy. time (0) interval t (tf. lookupException, tf. connectivityException, tf. extrapoationexception): continue angular = 4 * math. atan2 (trans [1], trans [0]) linear = 0.5 * math. sqrt (trans [0] ** 2 + trans [1] ** 2) turtle_vel.publish (turtlesim. msg. velocity (linear, angular) rate. sleep ()

Add the following nodes to the launch file:

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

Then, we can see two turtle instances, that is, the follow effect we first saw. 4. In many applications, it is necessary to add a frame. For example, in a world coordinate system, there is a laser scanning node, tf can help us replace the coordinates of the laser scan information with the global coordinates. 1. The coordinate frame in the frame structure tf is a tree structure, and the world coordinate system is the parent coordinate frame at the top. Other coordinate frames must be extended down. If we add a frame based on the above, we need to make the new frame a child frame of the three existing frames.

2. Create a fixed frame. We use turtle1 as the parent frame and create a new frame "carrot1 ". The Code is as follows (nodes/fixed_tf_broadcaster.py ):

#! /Usr/bin/env python import roslibroslib. load_manifest ('learning _ tf') import rospyimport tfif _ name _ = '_ main _': rospy. init_node ('My _ tf_broadcaster ') br = tf. transformBroadcaster () rate = rospy. rate (10.0) while not rospy. is_shutdown (): br. sendTransform (0.0, 2.0, 0.0), (0.0, 0.0, 0.0, 1.0), rospy. time. now (), "carrot1", "turtle1") # create a new frame. The parent frame is turtle1 and 2 meters away from the parent frame. sleep ()

Add a node to the launch file:

  <launch>    ...    <node pkg="learning_tf" type="fixed_tf_broadcaster.py"          name="broadcaster_fixed" />  </launch>

Run the program. We can see that the two turtles have the same effect as before. The newly added frame does not affect other frames. Open the nodes/turtle_tf_listener.py file and change turtle1 to carrot1:

(trans,rot) = self.tf.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

Re-run, now the relationship between turtles is changed:
3. Create a moving frame. The new frame we create is a fixed frame, which will not be changed during simulation, if we want to set a variable relationship between the carrot1 frame and the turtle1 frame, we can modify the Code as follows:

#!/usr/bin/env python  import roslibroslib.load_manifest('learning_tf')import rospyimport tfimport mathif __name__ == '__main__':    rospy.init_node('my_tf_broadcaster')    br = tf.TransformBroadcaster()    rate = rospy.Rate(10.0)    while not rospy.is_shutdown():        t = rospy.Time.now().to_sec() * math.pi        br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),                         (0.0, 0.0, 0.0, 1.0),                         rospy.Time.now(),                         "carrot1",                         "turtle1")        rate.sleep()

The position of carrot1 is now a trigonometric function for turtle1.

Code download: http://download.csdn.net/detail/hcx25909/5708199

----------------------------------------------------------------

You are welcome to repost my article.

Reprinted, please note: transferred from ancient-month

Http://blog.csdn.net/hcx25909

Continue to follow my blog