ROS开发技术的twist_mux功能包的原理和使用方法，灵活使用锁配置

twist_mux的原理类似于RTOS中的任务调度，需要为每个输入的话题设置优先级，不同话题也可以设置为同样的优先级，但是并不推荐这样做。除了优先级机制外，twist_mux还可以根据超时限制、外部锁话题（lock topics）来限制输入话题的选择。

我们在做机器人ROS开发时，常常会遇到相同速度控制消息的选择问题。比如说控制机器人移动的geometry_msgs::Twist消息，可以使用键盘节点发布，可以使用手柄节点发布，也可以在导航过程中由movebase发布，那么这些节点同时运行时，多个节点发布相同的速度控制话题，这个时候机器人就迷茫了，到底该听谁的呢？只能收到哪个数据就听谁的。

针对这样的问题，ROS提供了一种mux多路切换器——twist_mux，可以帮助我们切换到希望接收的数据源上。

—  原理 —

正如我们开篇描述的场景，当机器人接收到不同节点发布的速度控制消息时，该如何进行选择呢？

回想一下RTOS中的任务调度原则，每个任务都有自己的优先级，当有多个任务进入等待状态时，系统会选择优先级最高的任务执行，很多系统还支持同等优先级的任务，此时系统会按照时间片轮询的方式执行任务，也就是不偏不倚，平等对待相同优先级的任务。

twist_mux的原理类似于RTOS中的任务调度，需要为每个输入的话题设置优先级，不同话题也可以设置为同样的优先级，但是并不推荐这样做。

除了优先级机制外，twist_mux还可以根据超时限制、外部锁话题（lock topics）来限制输入话题的选择。

twist_mux功能包中的核心节点就是twist_mux，其输入、输出如下：

左侧输入的是多个geometry_msgs::Twist类型的话题，通过twist_mux的选择后，输出唯一的geometry_msgs::Twist话题。下方输入的话题就是用户动态配置选择机制的锁话题，话题的消息类型是Bool，正如锁一样，只有打开和关闭两种状态。

这里锁的概念可以理解为：通过限制不同优先级的输入源，达到控制输出的效果。

—  配置 —

twist_mux功能包的安装一句话就可以搞定：

sudo apt-get install ros-indigo-twist-mux

然后就可以运行多路切换器了：

roslaunch twist_mux twist_mux.launch

打印当前的话题列表可以看到：

再来看一下twist_mux.launch文件里边到底干了啥：

                                    

可以看到，在launch文件中配置了一些话题名参数，在启动twist_mux节点的过程中还加载了两个配置文件，twist_mux_topics.yaml对应的就是输入话题的配置，twist_mux_locks.yaml对应的是锁话题的配置。

1. twist_mux_topics.yaml

# Input topics handled/muxed.# For each topic:# - name    : name identifier to select the topic# - topic   : input topic of geometry_msgs::Twist type# - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead# - priority: priority in the range [0, 255]; the higher the more priority over other topicstopics:- name    : navigation topic   : nav_vel timeout : 0.5 priority: 10- name    : joystick topic   : joy_vel timeout : 0.5 priority: 100- name    : keyboard topic   : key_vel timeout : 0.5 priority: 90- name    : tablet topic   : tab_vel timeout : 0.5 priority: 100

该配置文件中包含了一个输入话题的配置列表，每个输入话题的配置项包含以下几个：

name：一个用户可读的命名，不是话题名，只用于调试显示；

topic：话题名，话题必须是geometry_msgs::Twist类型；

timeout：消息允许的超时限制，超过此时间仍然没有数据后，会切换到其他输入话题上，如果设置为0的话，相当于没有限制，会无限等待；

priority：输入话题的优先级，0~255之间，值越大优先级越高

2. twist_mux_locks.yaml

# Locks to stop the twist inputs.# For each lock:# - topic   : input topic that provides the lock; it must be of type std_msgs::Bool?!!!# - timeout : == 0.0 -> not used#              > 0.0 -> the lock is supposed to published at a certain frequency in order#                       to detect that the publisher is alive; the timeout in seconds allows#                       to detect that, and if the publisher dies we will enable the lock# - priority: priority in the range [0, 255], so all the topics with priority lower than it#             will be stopped/disabledlocks:- name    : pause topic   : pause_navigation timeout : 0.0 # Same priority as joystick control, so it'll not block it. priority: 100- name    : loop_closure topic   : stop_closing_loop timeout : 0.0 priority: 200- name    : joystick topic   : joy_priority timeout : 0.0 priority: 100

关于锁话题的配置也是一个列表，每一个子项都是一个锁话题的配置，配置项包含以下几个：

name：一个用户可读的命名，不是话题名，只用于调试显示；

topic：锁话题名，消息类型必须是std_msgs::Bool 类型；

timeout：锁话题需要按周期发布，所以超过时间限制后，会认为发布锁的节点掉线了，锁失效，类似于看门狗的功能；如果设置为0的话，则没有超时限制，持续有效；

priority：限制输入话题的优先级，0~255之间，在此值以下优先级的话题，会被上锁，开锁之前无法输出，以达到动态控制输出的效果。

—  实践 —

现在我们就来通过实践验证一下twist_mux的效果。

首先启动twist_mux节点：

roslaunch twist_mux twist_mux.launch

然后监听最终输出的速度控制消息：

rostopic echo /twist_mux/cmd_vel

1. 多输入源的选择

目前还没有任何输入，所以还不会有输出的消息。

接着我们就可以发布几个输入的消息了，先发布一个导航输出的速度消息：

rostopic pub -r 10 /nav_vel geometry_msgs/Twist "linear: x: 1.0 y: 0.0 z: 0.0angular: x: 0.0 y: 0.0 z: 0.0"

成功发布之后，输出监听并没有任何输出，这是因为在锁消息中，我们设置的最小优先级是100，而nav_vel的优先级在配置文件中设置的是10，自然被挡到了门外，没办法输出。

再来发布joy_vel消息：

rostopic pub -r 10 /joy_vel geometry_msgs/Twist "linear: x: 2.0 y: 0.0 z: 0.0angular: x: 0.0 y: 0.0 z: 0.0"

很快就可以在输出监听的终端中看到如下图所示的joy_vel的消息了：

此时有nav_vel和joy_vel两个输入消息，经过twist_mux的选择后，只输出了joy_vel消息。

那么如果我们想要输出nav_vel怎么办呢？

在nav_vel和joy_vel两个消息保持发布的状态下，重新打开一个窗口，关闭优先级门槛的限制：

rostopic pub /joy_priority std_msgs/Bool "data: false"

此时如果joy_vel消息停止或者超时的话，twist_mux就会自动切换到nav_vel输出，我们可以直接kiil掉joy_vel的话题发布，输出监听的终端就会显示nav_vel消息的数据了：

2. 停止某优先级以下的输入源

如果我们想要暂停输出，怎么办呢？锁消息中也设置了暂停nav_vel的话题：

rostopic pub /pause_navigation std_msgs/Bool "data: true"

现在nav_vel话题就暂停了，想要恢复的话，只需要再发布一次pause_navigation消息即可：

rostopic pub /pause_navigation std_msgs/Bool "data: false"

需要注意的是，/pause_navigation话题设置的优先级和joy_priority是一样的，所以这里的暂停无法暂停优先级100及以上的话题，所以并不会影响joy_vel消息的转发。

我们还在锁配置中设置了一个优先级为200的控制锁，一旦我们生效这个锁，优先级200以下的话题都会停止转发：

rostopic pub /stop_closing_loop std_msgs/Bool "data: true"

现在所有话题都无法通过twist_mux输出了。

OK，现在我们应该已经明白了twist_mux功能包的原理和使用方法，灵活使用锁配置，可以让我们轻松控制多个输入源的切换。