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-getinstallros-indigo-twist-mux

然后就可以運行多路切換器了：

roslaunchtwist_muxtwist_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::Twisttype#- timeout : timeoutinseconds to start discarding old messages, and use 0.0 speed instead#- priority: priorityinthe range [0, 255]; the higher the more priority over other topicstopics:-name : navigationtopic : nav_veltimeout : 0.5priority: 10-name : joysticktopic : joy_veltimeout : 0.5priority: 100-name : keyboardtopic : key_veltimeout : 0.5priority: 90-name : tablettopic : tab_veltimeout : 0.5priority: 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 oftypestd_msgs::Bool?!!!#- timeout : == 0.0 -> not used# > 0.0 -> the lock is supposed to published at a certain frequencyinorder# to detect that the publisher is alive; the timeoutinseconds allows# to detect that, andifthe publisher dies we willenablethe lock#- priority: priorityinthe range [0, 255], so all the topics with priority lower than it# will be stopped/disabledlocks:-name : pausetopic : pause_navigationtimeout : 0.0#Same priority as joystick control, so it'll not block it.priority: 100-name : loop_closuretopic : stop_closing_looptimeout : 0.0priority: 200-name : joysticktopic : joy_prioritytimeout : 0.0priority: 100

關于鎖話題的配置也是一個列表，每一個子項都是一個鎖話題的配置，配置項包含以下幾個：

name：一個用戶可讀的命名，不是話題名，只用于調試顯示；

topic：鎖話題名，消息類型必須是std_msgs::Bool類型；

timeout：鎖話題需要按周期發布，所以超過時間限制后，會認為發布鎖的節點掉線了，鎖失效，類似于看門狗的功能；如果設置為0的話，則沒有超時限制，持續有效；

priority：限制輸入話題的優先級，0~255之間，在此值以下優先級的話題，會被上鎖，開鎖之前無法輸出，以達到動態控制輸出的效果。

— 實踐—

現在我們就來通過實踐驗證一下twist_mux的效果。

首先啟動twist_mux節點：

roslaunchtwist_muxtwist_mux.launch

然后監聽最終輸出的速度控制消息：

rostopicecho/twist_mux/cmd_vel

1. 多輸入源的選擇

目前還沒有任何輸入，所以還不會有輸出的消息。

接著我們就可以發布幾個輸入的消息了，先發布一個導航輸出的速度消息：

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

成功發布之后，輸出監聽并沒有任何輸出，這是因為在鎖消息中，我們設置的最小優先級是100，而nav_vel的優先級在配置文件中設置的是10，自然被擋到了門外，沒辦法輸出。

再來發布joy_vel消息：

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

很快就可以在輸出監聽的終端中看到如下圖所示的joy_vel的消息了：

此時有nav_vel和joy_vel兩個輸入消息，經過twist_mux的選擇后，只輸出了joy_vel消息。

那么如果我們想要輸出nav_vel怎么辦呢？

在nav_vel和joy_vel兩個消息保持發布的狀態下，重新打開一個窗口，關閉優先級門檻的限制：

rostopicpub /joy_priority std_msgs/Bool"data: false"

此時如果joy_vel消息停止或者超時的話，twist_mux就會自動切換到nav_vel輸出，我們可以直接kiil掉joy_vel的話題發布，輸出監聽的終端就會顯示nav_vel消息的數據了：

2. 停止某優先級以下的輸入源

如果我們想要暫停輸出，怎么辦呢？鎖消息中也設置了暫停nav_vel的話題：

rostopicpub /pause_navigation std_msgs/Bool"data: true"

現在nav_vel話題就暫停了，想要恢復的話，只需要再發布一次pause_navigation消息即可：

rostopicpub /pause_navigation std_msgs/Bool"data: false"

需要注意的是，/pause_navigation話題設置的優先級和joy_priority是一樣的，所以這里的暫停無法暫停優先級100及以上的話題，所以并不會影響joy_vel消息的轉發。

我們還在鎖配置中設置了一個優先級為200的控制鎖，一旦我們生效這個鎖，優先級200以下的話題都會停止轉發：

rostopicpub /stop_closing_loop std_msgs/Bool"data: true"

現在所有話題都無法通過twist_mux輸出了。

OK，現在我們應該已經明白了twist_mux功能包的原理和使用方法，靈活使用鎖配置，可以讓我們輕松控制多個輸入源的切換。