跟随

效果介绍¶

界面上有两只小乌龟：

小乌龟A
小乌龟B
小乌龟B跟随着小乌龟A走
小乌龟A通过键盘控制移动

分析¶

界面上需要显示两只小乌龟
小乌龟A通过键盘运动
小乌龟B通过代码控制来跟随小乌龟A运动

坐标系构建¶

小乌龟A的坐标系
小乌龟B的坐标系

可以通过订阅广播获得两只小乌龟的位置信息.

TF开发流程¶

坐标系构建¶

将显示界面定义为世界坐标系
将小乌龟A定义为自身坐标系
将小乌龟B定义为自身坐标系

广播发送自身位置¶

小乌龟A¶

from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler


def pose_callback(msg):
    if not isinstance(msg, Pose):
        return

    x = msg.x
    y = msg.y
    # 小乌龟Z轴转动的角度, roll: x   pitch: y    yaw:z
    theta = msg.theta

    # 实时发布位置信息到TF工具
    # 位置
    translation = (x, y, 0)
    # 姿态 tf工具是用四元素来描述姿态信息 将 rpy欧拉角描述方式转换为 四元素描述方式
    rotation = quaternion_from_euler(0, 0, theta)

    broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle_a", "world")

小乌龟B¶

实现和小乌龟A类似

收听者获取坐标关系¶

以小乌龟B为参考系

以小乌龟A为参考系

# 获取相对位置信息的listener
listener = TransformListener()

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    # //实时查看turtle_a 在 turtle_b的坐标系中的位置
    #
    # // target_frame: 坐标系，参考坐标系
    # // source_frame: 求解的坐标系
    # // 想知道 source_frame在 target_frame的位置信息
    # // time: 0获得最近我和你的相对位置
    try:
        transform = listener.lookupTransform("turtle_a", "turtle_b", rospy.Time())
    except:
        rate.sleep()
        continue
    # 获取位置
    x, y, z = transform[0]
    # # 获取姿态 (四元素) -> rpy
    quat = transform[1]
    roll, pitch, yaw = euler_from_quaternion(quat)

运动规划¶

以小乌龟B为参考系

以小乌龟A为参考系

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

twist = Twist()
twist.linear.x = 0.6 * distance
twist.angular.z = 6 * angular
publisher.publish(twist)

总结优化¶

实现的内容¶

小乌龟A节点¶

订阅小乌龟A的位姿
广播小乌龟A和界面的坐标关系

小乌龟B节点¶

产生小乌龟B
订阅小乌龟B的位姿
广播小乌龟B和界面的坐标关系
接收小乌龟A和B的坐标关系
控制小乌龟B实现追随功能

优化策略¶

小乌龟节点¶

杀死小乌龟
产生小乌龟
订阅小乌龟的位姿
广播小乌龟和界面的坐标关系

追随功能节点¶

接收小乌龟A和B的坐标关系
控制小乌龟实现追随功能

launch文件¶

配置两个小乌龟节点，参数不同
配置追随功能节点，参数配置