文章目录

• 机器人中的坐标变换
• 坐标变换实验
• TF坐标系广播与监听

机器人中的坐标变换

  TF功能包的作用举例如下：

• 五秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？
• 机器人抓取的物体相对于机器人中心坐标系的位置在哪里？
• 机器人中心坐标系相对于全局坐标系的位置在哪里？

  TF坐标变换实现方法如下：

1. 广播TF变换。
2. 监听TF变换。

  机器人系统中有着繁杂的坐标系：

  下图显示了移动机器人的本体坐标系与雷达坐标系：

  坐标系之间的数据变换如下：

坐标变换实验

  在终端上输入如下的命令：

$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key
$ rosrun tf view_frames

在终端上使用方向键控制小乌龟，然后另外一只乌龟将会跟踪这个小乌龟：

rosrun tf view_frames命令会在当前文件夹下生成frames.pdf：

TF坐标系广播与监听

  创建功能包：

$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

  创建TF广播器的流程如下：

1. 定义TF广播器(TransformBroadcaster)。
2. 创建坐标变换值。
3. 发布坐标变换(sendTransform)。

  turtle_tf_broadcaster.cpp如下：

#include 
#include 
#include 

std::string turtle_name;

void poseCallback ( const turtlesim::PoseConstPtr& msg ) {
    static tf::TransformBroadcaster br; 
    
    tf::Transform transform;
    transform.setOrigin ( tf::Vector3 ( msg->x, msg->y, 0.0 ) );
    tf::Quaternion q;
    q.setRPY ( 0, 0, msg->theta );
    transform.setRotation ( q );
    
    br.sendTransform ( tf::StampedTransform ( transform, ros::Time::now(), "world", turtle_name ) );
}

int main ( int argc, char** argv ) {
    ros::init ( argc, argv, "my_tf_broadcaster" ); 

    
    if ( argc != 2 ) {
        ROS_ERROR ( "need turtle name as argument" );
        return -1;
    }

    turtle_name = argv[1];
    
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe ( turtle_name + "/pose", 10, poseCallback );
    ros::spin(); 
    return 0;
}

  创建TF监听器的流程如下：

1. 定义TF监听器(TransformListener)。
2. 查找坐标变换(waitForTransform、lookupTransform)。

  turtle_tf_listener.cpp如下：

#include 
#include 
#include 
#include 

int main ( int argc, char** argv ) {
    ros::init ( argc, argv, "my_tf_listener" ); 
    ros::NodeHandle node; 
    
    ros::service::waitForService ( "/spawn" );
    ros::ServiceClient add_turtle = node.serviceClient ( "/spawn" );
    turtlesim::Spawn srv;
    add_turtle.call ( srv );
    
    ros::Publisher turtle_vel = node.advertise ( "/turtle2/cmd_vel", 10 );
    
    tf::TransformListener listener;
    ros::Rate rate ( 10.0 );

    while ( node.ok() ) {
        tf::StampedTransform transform; 

        try { 
            listener.waitForTransform ( "/turtle2", "/turtle1", ros::Time ( 0 ), ros::Duration ( 3.0 ) );
            listener.lookupTransform ( "/turtle2", "/turtle1", ros::Time ( 0 ), transform );
        } catch ( tf::TransformException &ex ) {
            ROS_ERROR ( "%s", ex.what() );
            ros::Duration ( 1.0 ).sleep();
            continue;
        }

        
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2 ( transform.getOrigin().y(),                   transform.getOrigin().x() );
        vel_msg.linear.x = 0.5 * sqrt ( pow ( transform.getOrigin().x(), 2 ) +                 pow ( transform.getOrigin().y(), 2 ) );
        turtle_vel.publish ( vel_msg );
        rate.sleep();
    }

    return 0;
}

  修改catkin_ws/src/learning_tf目录下的CMakeLists.txt：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

  编译并运行代码：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
$ rosrun learning_tf turtle_tf_listener
$ rosrun turtlesim turtle_teleop_key

可以看到两个小乌龟像上面一样追逐。
  以上的代码也可以使用python来实现。在catkin_ws/src/learning_tf/scripts目录下，创建一个名为turtle_tf_broadcaster.py的文件：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求“/show_person”服务，服务数据类型为“learning_service::Person”
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()

再创建一个名为turtle_tf_listener.py的文件：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求“/show_person”服务，服务数据类型为“learning_service::Person”
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()

  执行以上的python代码可以使用launch文件：