ROS開發(fā)技術(shù)的twist_mux功能包的原理和使用方法，靈活使用鎖配置

twist_mux的原理類似于RTOS中的任務(wù)調(diào)度，需要為每個(gè)輸入的話題設(shè)置優(yōu)先級(jí)，不同話題也可以設(shè)置為同樣的優(yōu)先級(jí)，但是并不推薦這樣做。除了優(yōu)先級(jí)機(jī)制外，twist_mux還可以根據(jù)超時(shí)限制、外部鎖話題（lock topics）來限制輸入話題的選擇。

我們?cè)谧?a target="_blank">機(jī)器人ROS開發(fā)時(shí)，常常會(huì)遇到相同速度控制消息的選擇問題。比如說控制機(jī)器人移動(dòng)的geometry_msgs::Twist消息，可以使用鍵盤節(jié)點(diǎn)發(fā)布，可以使用手柄節(jié)點(diǎn)發(fā)布，也可以在導(dǎo)航過程中由movebase發(fā)布，那么這些節(jié)點(diǎn)同時(shí)運(yùn)行時(shí)，多個(gè)節(jié)點(diǎn)發(fā)布相同的速度控制話題，這個(gè)時(shí)候機(jī)器人就迷茫了，到底該聽誰的呢？只能收到哪個(gè)數(shù)據(jù)就聽誰的。

針對(duì)這樣的問題，ROS提供了一種mux多路切換器——twist_mux，可以幫助我們切換到希望接收的數(shù)據(jù)源上。

— 原理—

正如我們開篇描述的場(chǎng)景，當(dāng)機(jī)器人接收到不同節(jié)點(diǎn)發(fā)布的速度控制消息時(shí)，該如何進(jìn)行選擇呢？

回想一下RTOS中的任務(wù)調(diào)度原則，每個(gè)任務(wù)都有自己的優(yōu)先級(jí)，當(dāng)有多個(gè)任務(wù)進(jìn)入等待狀態(tài)時(shí)，系統(tǒng)會(huì)選擇優(yōu)先級(jí)最高的任務(wù)執(zhí)行，很多系統(tǒng)還支持同等優(yōu)先級(jí)的任務(wù)，此時(shí)系統(tǒng)會(huì)按照時(shí)間片輪詢的方式執(zhí)行任務(wù)，也就是不偏不倚，平等對(duì)待相同優(yōu)先級(jí)的任務(wù)。

twist_mux的原理類似于RTOS中的任務(wù)調(diào)度，需要為每個(gè)輸入的話題設(shè)置優(yōu)先級(jí)，不同話題也可以設(shè)置為同樣的優(yōu)先級(jí)，但是并不推薦這樣做。

除了優(yōu)先級(jí)機(jī)制外，twist_mux還可以根據(jù)超時(shí)限制、外部鎖話題（lock topics）來限制輸入話題的選擇。

twist_mux功能包中的核心節(jié)點(diǎn)就是twist_mux，其輸入、輸出如下：

左側(cè)輸入的是多個(gè)geometry_msgs::Twist類型的話題，通過twist_mux的選擇后，輸出唯一的geometry_msgs::Twist話題。下方輸入的話題就是用戶動(dòng)態(tài)配置選擇機(jī)制的鎖話題，話題的消息類型是Bool，正如鎖一樣，只有打開和關(guān)閉兩種狀態(tài)。

這里鎖的概念可以理解為：通過限制不同優(yōu)先級(jí)的輸入源，達(dá)到控制輸出的效果。

— 配置—

twist_mux功能包的安裝一句話就可以搞定：

sudo apt-getinstallros-indigo-twist-mux

然后就可以運(yùn)行多路切換器了：

roslaunchtwist_muxtwist_mux.launch

打印當(dāng)前的話題列表可以看到：

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

可以看到，在launch文件中配置了一些話題名參數(shù)，在啟動(dòng)twist_mux節(jié)點(diǎn)的過程中還加載了兩個(gè)配置文件，twist_mux_topics.yaml對(duì)應(yīng)的就是輸入話題的配置，twist_mux_locks.yaml對(duì)應(yīng)的是鎖話題的配置。

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

該配置文件中包含了一個(gè)輸入話題的配置列表，每個(gè)輸入話題的配置項(xiàng)包含以下幾個(gè)：

name：一個(gè)用戶可讀的命名，不是話題名，只用于調(diào)試顯示；

topic：話題名，話題必須是geometry_msgs::Twist類型；

timeout：消息允許的超時(shí)限制，超過此時(shí)間仍然沒有數(shù)據(jù)后，會(huì)切換到其他輸入話題上，如果設(shè)置為0的話，相當(dāng)于沒有限制，會(huì)無限等待；

priority：輸入話題的優(yōu)先級(jí)，0~255之間，值越大優(yōu)先級(jí)越高

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

關(guān)于鎖話題的配置也是一個(gè)列表，每一個(gè)子項(xiàng)都是一個(gè)鎖話題的配置，配置項(xiàng)包含以下幾個(gè)：

name：一個(gè)用戶可讀的命名，不是話題名，只用于調(diào)試顯示；

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

timeout：鎖話題需要按周期發(fā)布，所以超過時(shí)間限制后，會(huì)認(rèn)為發(fā)布鎖的節(jié)點(diǎn)掉線了，鎖失效，類似于看門狗的功能；如果設(shè)置為0的話，則沒有超時(shí)限制，持續(xù)有效；

priority：限制輸入話題的優(yōu)先級(jí)，0~255之間，在此值以下優(yōu)先級(jí)的話題，會(huì)被上鎖，開鎖之前無法輸出，以達(dá)到動(dòng)態(tài)控制輸出的效果。

— 實(shí)踐—

現(xiàn)在我們就來通過實(shí)踐驗(yàn)證一下twist_mux的效果。

首先啟動(dòng)twist_mux節(jié)點(diǎn)：

roslaunchtwist_muxtwist_mux.launch

然后監(jiān)聽最終輸出的速度控制消息：

rostopicecho/twist_mux/cmd_vel

1. 多輸入源的選擇

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

接著我們就可以發(fā)布幾個(gè)輸入的消息了，先發(fā)布一個(gè)導(dǎo)航輸出的速度消息：

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

成功發(fā)布之后，輸出監(jiān)聽并沒有任何輸出，這是因?yàn)樵阪i消息中，我們?cè)O(shè)置的最小優(yōu)先級(jí)是100，而nav_vel的優(yōu)先級(jí)在配置文件中設(shè)置的是10，自然被擋到了門外，沒辦法輸出。

再來發(fā)布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"

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

此時(shí)有nav_vel和joy_vel兩個(gè)輸入消息，經(jīng)過twist_mux的選擇后，只輸出了joy_vel消息。

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

在nav_vel和joy_vel兩個(gè)消息保持發(fā)布的狀態(tài)下，重新打開一個(gè)窗口，關(guān)閉優(yōu)先級(jí)門檻的限制：

rostopicpub /joy_priority std_msgs/Bool"data: false"

此時(shí)如果joy_vel消息停止或者超時(shí)的話，twist_mux就會(huì)自動(dòng)切換到nav_vel輸出，我們可以直接kiil掉joy_vel的話題發(fā)布，輸出監(jiān)聽的終端就會(huì)顯示nav_vel消息的數(shù)據(jù)了：

2. 停止某優(yōu)先級(jí)以下的輸入源

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

rostopicpub /pause_navigation std_msgs/Bool"data: true"

現(xiàn)在nav_vel話題就暫停了，想要恢復(fù)的話，只需要再發(fā)布一次pause_navigation消息即可：

rostopicpub /pause_navigation std_msgs/Bool"data: false"

需要注意的是，/pause_navigation話題設(shè)置的優(yōu)先級(jí)和joy_priority是一樣的，所以這里的暫停無法暫停優(yōu)先級(jí)100及以上的話題，所以并不會(huì)影響joy_vel消息的轉(zhuǎn)發(fā)。

我們還在鎖配置中設(shè)置了一個(gè)優(yōu)先級(jí)為200的控制鎖，一旦我們生效這個(gè)鎖，優(yōu)先級(jí)200以下的話題都會(huì)停止轉(zhuǎn)發(fā)：

rostopicpub /stop_closing_loop std_msgs/Bool"data: true"

現(xiàn)在所有話題都無法通過twist_mux輸出了。

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