ROS控制多台机器人实现多机协同

1. 实现思路

1.1 两台机器人跟随

首先先来缕一缕思路，如果要实现turtlebot2跟随turtlebot1，则需要知道两台机器人之间的位置和姿态的关系。

具体如何获得机器人各自的 位姿坐标 关系，可以通过SLAM建图、里程计、全局相机、动作捕捉设备等等，获取坐标后可以通过广播的方式发给ROS中的 TF工具。之后通过订阅 TF工具 中两机器人的关系即可。

之后通过两机器人之间的位姿关系可获得两机器人的 距离 和 姿态角差值，通过PID控制分别发布 速度 和 角速度 指令，实现机器人的跟随控制。

1.2 多台机器人编队

如 1.1 中思路，可分别获取多机器人间的坐标关系，此时可以通过 TF工具 中的 静态坐标关系广播 获取所需队形各自位置的坐标，之后如上节一样，每台机器人去跟随自己所需到达的对应坐标即可。

2 TF工具的使用

• TF工具的讲解见：ROS学习记录（5）——TF工具的使用与练习

2.1 什么是TF

TF是Transformations Frames的缩写。在ROS中，是一个工具，提供了坐标转换等方面的功能。

说白了TF工具就是帮助你完成坐标转换的一个工具，有了它就不用去计算各个坐标系之间的转换了，学过机器人学的应该会对此感受颇深。

2.2 TF的构成

在ROS中，TF工具包包含了三块内容：Broadcaster，Listener，TF转换工具

ROS中提供的是TF转换工具。转换是通过两个部分来完成的。

• Broadcaster 负责向TF工具广播 参照物 和 自己的位置 关系
• Listener 负责向TF工具查看 想要知道的 两个物体间的相对坐标

TF工具底层是通过向量来去实现的。

2.3 向TF工具广播发送自己位置

部分代码

from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler

broadcaster = TransformBroadcaster()


def pose_callback(msg):
	if not isinstance(msg, Pose):
        return
    # 机器人在参考坐标系的位置msg.x和msg.y，姿态msg.theta
	translation = (msg.x, msg.y, 0)
	rotation = quaternion_from_euler(0, 0, msg.theta)		# 欧拉角 ->  四元素
	
	broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "child", "parent")		# 发广播

其中 quaternion_from_euler 将欧拉角转换为四元素：欧拉角，旋转矩阵。其中欧拉角：roll：x、pitch：y、yaw：z。

sendTransform 中传入的几组数分别为：

• translation：描述位置
• rotation：通过四元素来描述姿态
• Time().now()：time，打上时间戳
• "child"：子坐标系（机器人坐标系）
• "parent"：父坐标系（机器人的参考坐标系）

2.4 向TF工具收听获取坐标关系

部分代码

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

rate = rospy.Rate(10)
while not rospy.is_shutdown():

    try:
        transform = listener.lookupTransform("target_frame", "source_frame", rospy.Time())
    except:
        rate.sleep()
        continue
    
    x, y, z = transform[0] 		# 获取位置
    quat = transform[1]		# 获取姿态 (四元素) -> rpy
    roll, pitch, yaw = euler_from_quaternion(quat)

lookupTransform 中传入的几组数分别为：

• "target_frame"：参考坐标系
• "source_frame"：求解的坐标系
• rospy.Time()：获得最近的相对位置
  返回的数据为：
• 位置：[x, y, z]
• 姿态：[roll, pitch, yaw, w]

3 通过turtlesim实现

待补充

4. 如何同时运行多台Turtlebot

4.1 通过命名空间

启动launch时，前面加上ROS_NAMESPACE='xx'，这样的话启动的机器人应该就自带一个命名空了，它所接受的信息都要带xx/前缀，这样可以做到多个机器人分别控制，如：

ROS_NAMESPACE=robot1 roslaunch turtlebot3_bringup turtlebot3_robot.launch --screen
ROS_NAMESPACE=robot2 roslaunch turtlebot3_bringup turtlebot3_robot.launch --screen
ROS_NAMESPACE=robot3 roslaunch turtlebot3_bringup turtlebot3_robot.launch --screen

之后再运行rosrun前也加上命名空间即可对应不同的机器人了

ROS_NAMESPACE=robot1 rosrun turtlebot3_teleop turtlebot3_teleop_key
ROS_NAMESPACE=robot2 rosrun turtlebot3_teleop turtlebot3_teleop_key
ROS_NAMESPACE=robot3 rosrun turtlebot3_teleop turtlebot3_teleop_key

5. Demo——双机跟随

这里放上@清晨的光明的demo

• 代码地址：
  Github
  码云