案例一

TF开发流程¶

1. 建立Broadcaster，将小乌龟1坐标发送给TF工具

void callback(const turtlesim::Pose::ConstPtr &message, tf::TransformBroadcaster broadcaster) {
    tf::Transform transform;
    //设置位置
    transform.setOrigin(tf::Vector3(message->x, message->y, 0));
    //设置姿态
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, message->theta);
    transform.setRotation(quaternion);
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "turtle1"));
}

我们在小乌龟pose回调中，得到小乌龟相对于窗体的坐标信息(message)。

broadcaster在发送相对位置信息时，需要给定两个类型的数据：

• 在相对环境中的 坐标（origin），包含x,y,z坐标
• 在相对环境中的姿态 (rotation)，包含x,y,z方向的转动情况

在发送数据时，要标明谁相对谁的位置。参照物我们认为是父坐标。

2. 建立Broadcaster，将小乌龟2坐标发送给TF工具

参考第1步骤

3. 建立Listener，通过查看TF工具获得小乌龟1和小乌龟2间的相对坐标

tf::TransformListener listener;
ros::Rate rate(10);
while (ros::ok()) {
    tf::StampedTransform transform;
    try {
        listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    } catch (exception e) {
        ROS_INFO_STREAM(e.what());
        rate.sleep();
        continue;
    }
    tf::Vector3 &origin = transform.getOrigin();
    double x = origin.x();
    double y = origin.y();

    const tf::Quaternion &quaternion = transform.getRotation();
    double theta = quaternion.z();
}

转换监听器listener可以从TF工具中，获得想要的两个物体间的相对坐标。

前一个参数是作为参照物存在的，坐标和姿态都是(0,0,0)

第二个参数是相对于第一个参数的参照物的坐标和姿态。

4. 运动规划

double distance = sqrt(pow(x, 2) + pow(y, 2));
double angular = atan2(y, x);

geometry_msgs::Twist msg;
msg.linear.x = distance * 0.5;
msg.angular.z = angular * 3;
publisher.publish(msg);

完整代码

乌龟一

#!/usr/bin/env python3
# coding:utf-8
import rospy
from turtlesim.msg import Pose
from tf import TransformBroadcaster
from tf.transformations import quaternion_from_euler

def callBack(pose):
    # 位置
    translation = (pose.x,pose.y,0)
    # 姿态的四元数
    rotation = quaternion_from_euler(0,0,pose.theta)
    # 发布位置给tf
    broadcaster.sendTransform(translation,rotation,rospy.Time().now(),'turtle1','world')

if __name__ == '__main__':
    # 创建节点
    rospy.init_node("node_turtle1")
    #------------------------ 创建tf发布者 ------------------------#
    broadcaster = TransformBroadcaster()
    #------------------------ 订阅位置 ------------------------#
    subscriber = rospy.Subscriber('/turtle1/pose', Pose, callback=callBack)
    # 阻塞
    rospy.spin()

乌龟二

#!/usr/bin/env python3
# coding:utf-8
import rospy
from turtlesim.srv import Spawn, SpawnRequest
from turtlesim.msg import Pose
from tf import TransformBroadcaster,TransformListener
from tf.transformations import quaternion_from_euler
import math
from geometry_msgs.msg import Twist

def callBack(pose):
    # 位置
    translation = (pose.x,pose.y,0)
    # 姿态的四元数
    rotation = quaternion_from_euler(0,0,pose.theta)
    # 发布位置给tf
    broadcaster.sendTransform(translation,rotation,rospy.Time().now(),'turtle2','world')

if __name__ == '__main__':
    # 创建节点
    rospy.init_node("node_turtle2")
    # ------------------------ 创建小乌龟 ------------------------#
    proxy = rospy.ServiceProxy('/spawn', Spawn)
    proxy.wait_for_service()

    request = SpawnRequest()
    request.x = 1.0
    request.y = 1.0
    request.theta = 0
    request.name = 'turtle2'
    # 调用服务
    proxy.call(request)
    #------------------------ 创建tf发布者 ------------------------#
    broadcaster = TransformBroadcaster()
    #------------------------ 订阅小乌龟2的位置 ------------------------#
    subscriber = rospy.Subscriber('/turtle2/pose', Pose, callback=callBack)

    #------------------------ 发布者 ------------------------#
    publisher = rospy.Publisher('/turtle2/cmd_vel', Twist)
    #------------------------ 监听tf  移动小乌龟2 ------------------------#
    # 创建监听者
    listener = TransformListener()
    while not rospy.is_shutdown():
        try:
            # 获取turtle1在turtle2中的表示
            pose,rotation = listener.lookupTransform('turtle2','turtle1',rospy.Time(0))
            # 获取x y
            x = pose[0]
            y = pose[1]
            # 求距离
            dst = math.sqrt(math.pow(x,2)+math.pow(y,2))
            # 求角度
            angule = math.atan2(y,x)

            data = Twist()
            data.linear.x = dst*0.6
            data.angular.z = angule*6
            # 发布控制小乌龟移动
            publisher.publish(data)
        except:
            pass
    # 阻塞
    rospy.spin()