ROS技術點滴 —— twist_mux多路切換器

來自專欄 古月居

我們在做機器人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> <arg name="joy_vel_in" default="input_joy/cmd_vel"/> <arg name="joy_vel_out" default="joy_vel"/> <arg name="cmd_vel_out" default="twist_mux/cmd_vel"/> <arg name="config_locks" default="$(find twist_mux)/config/twist_mux_locks.yaml"/> <arg name="config_topics" default="$(find twist_mux)/config/twist_mux_topics.yaml"/> <arg name="config_joy" default="$(find twist_mux)/config/joystick.yaml"/> <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen"> <remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/> <rosparam file="$(arg config_locks)" command="load"/> <rosparam file="$(arg config_topics)" command="load"/> </node> <node pkg="twist_mux" type="twist_marker" name="twist_marker"> <remap from="twist" to="$(arg cmd_vel_out)"/> <remap from="marker" to="twist_marker"/> </node> <node pkg="twist_mux" type="joystick_relay.py" name="joystick_relay" output="screen"> <remap from="joy_vel_in" to="$(arg joy_vel_in)"/> <remap from="joy_vel_out" to="$(arg joy_vel_out)"/> <rosparam file="$(arg config_joy)" command="load"/> </node></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 itll 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功能包的原理和使用方法，靈活使用鎖配置，可以讓我們輕鬆控制多個輸入源的切換。

更多內容可關注微信公眾號：古月居 (guyue_home)

或訪問古月居網站：

古月居 - 怕什麼真理無窮，進一寸有一寸的歡喜?

www.guyuehome.com

推薦閱讀：