ROS 功能包：yocs_cmd_vel_mux 多路復用速度控制(機器人速度控制切換)
-
在我們控制一個移動機器人運動時，可能會遇到如下場景：自研的移動機器人在自動導航的過程中突然迷路要撞牆了，一場車禍馬上就要發生，這時候，我們就會很希望能夠通過無線手柄或者鍵盤去控制小車緊急停車，讓小車改邪歸正，迷途知返。想要實現這個功能，就需要用到多路輸入復用控制，即把多種速度控制信號收集起來，並按照優先級發給小車，覆蓋掉自主導航的速度控制消息。
-
幸運的是，完善的ROS生態中已經給我們提供了這個名為
yocs_cmd_vel_mux的功能包。
一、yocs_cmd_vel_mux 工作示意圖
- 我們需要在配置文件中對該功能包接收的話題和優先級進行配置，它可以接收多個話題並將其基於優先級發送，處理流程示意如下：
需要注意的是，只要高優先級話題當前有發布就會覆蓋低優先級的話題，因此可以通過讓高優先級信息源持續發送速度0的信息來控制移動機器人停下。
二、安裝與配置
git地址：https://github.com/yujinrobot/yujin_ocs.git
注意要選擇與你的ROS版本對應的分支。
將yocs_cmd_vel_mux包的源文件放到ROS工作目錄的源文件夾中，（如gazebo_test_ws/src），然后使用 catkin_make 進行編譯。
2.1 示例使用 16th_SmartCar-Competition程序來演示
16th_SmartCar-Competition 下載地址
首先配置 yocs_cmd_vel_mux/param/example.yaml （注釋也比較清楚）
subscribers:
- name: "Default input" # 數據源名稱
topic: "/cmd_vel" # 提供cmd_vel的話題名稱
timeout: 0.1 # 某話題超過該時間沒有新的輸入則認為該輸入話題不活躍
priority: 0 # 優先級，范圍為（0-MAX_INT）數字越大優先級越高。
short_desc: "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here"
- name: "Keyboard operation"
topic: "/mykeyop"
timeout: 0.1
priority: 10
publisher: "/cmd_vel"
然后在 src/race_navigation/launch/teb_base.launch 中引入 standalone.launch
<launch>
<include file="$(find yocs_cmd_vel_mux)/launch/standalone.launch"/>
...
</launch>
根據 example.yaml中的配置，可以通過對 mykeyop 話題發布消息來控制小車
demo.py如下，通過運行這個demo.py即可讓導航中的小車后退。
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import os
import rospy
from geometry_msgs.msg import Twist
rospy.init_node('partrol')
pubcmd = rospy.Publisher('/mykeyop', Twist, queue_size=10)
# 發布的頻率為10Hz
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# 速度信息
move_cmd = Twist()
move_cmd.linear.x = -1.0 # 讓小車速度為 -1.0 即讓小車后退
move_cmd.linear.y = 0
pubcmd.publish(move_cmd)
rate.sleep()
2.2 使用ROS小烏龜turtlesim演示
ros_tutorials melodic [下載地址](https://github.com/ros/ros_tutorials/tree/melodic-devel
只需要下載turtlesim即可，然后將其放到工作目錄 src下，如（catkin_ws/src）catkin_make編譯。
默認安裝ros都會自動安裝小烏龜，如果沒有安裝 sudo apt-get install ros-$(rosversion -d)-turtlesim
teleop_twist_keyboard(鍵盤控制): sudo apt-get install ros-noetic-teleop-twist-keyboard
首先配置 yocs_cmd_vel_mux/param/example.yaml （注釋也比較清楚）
subscribers:
- name: "Default input"
topic: "input/default"
timeout: 0.1
priority: 0
short_desc: "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here"
- name: "Joystick control"
topic: "input/joystick"
timeout: 0.1
priority: 1
- name: "Keyboard operation"
topic: "input/keyop"
timeout: 0.1
priority: 2
publisher: "output/cmd_vel"
然后在 src/yocs_cmd_vel_mux/launch中新建 test.launch文件如下，文件中先開啟了cmd_vel_mux，然后運行turtlesim_node節點，最后開啟了turtle_teleop_key鍵盤控制節點：
<launch>
<include file="$(find yocs_cmd_vel_mux)/launch/standalone.launch"/>
<node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" >
<remap from="/turtle1/cmd_vel" to="/yocs_cmd_vel_mux/output/cmd_vel" />
</node>
<node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen">
<remap from="/turtle1/cmd_vel" to="/yocs_cmd_vel_mux/input/keyop" />
</node>
</launch>
launch文件中將turtlesim_node節點訂閱的 /turtle1/cmd_vel 消息重映射到mux的輸出 /yocs_cmd_vel_mux/output/cmd_vel 上。將 turtle_teleop_key 發布的消息 /turtle1/cmd_vel 重映射到mux的輸入 /yocs_cmd_vel_mux/input/keyop 上。要注意launch文件中remap的用法。如果該屬性在頂層， 即作為 launch 元素的子元素出現， 重映射將會應用到所有的后續節點。這些重映射元素也可以作為一個節點元素的子元素，如：
<node node-attributes>
<remap from="original-name" to="new-name"/>
</node>
在這種情況下，給出的重映射只應用於其所在的節點。
cmd_vel_mux的作用就像一個選擇開關，接收多個輸入信號，只輸出一個結果：
cmd_vel_mux會發布下面兩個消息，分別是輸出速度（名稱可以在配置文件中更改）和mux的當前選擇：
output/cmd_vel(geometry_msgs/Twist)：Multiplexed output. Incoming velocity commands from the active source are republished on this topic. The topic name is specified on the configuration file.active(std_msgs/String)：The active input at each moment, or idle if nobody is commanding the robot. Latched topic
輸入命令 roslaunch yocs_cmd_vel_mux test.launch 運行測試文件，然后打開一個新終端輸入下面的指令作為默認控制信號：
rostopic pub -r 10 /yocs_cmd_vel_mux/input/default geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.5}}'
使用rostopic echo查看一下mux的輸出，可以看到正在執行默認控制指令，小海龜以0.5rad/s的角速度旋轉：
active為：Default input
接着在turtle_teleop_key的窗口中按下方向鍵，可以看到小海龜改由鍵盤控制，active變為Keyboard operation：
輸出速度output/cmd_vel也變為鍵盤控制的速度：
三、原理
查看了一下源代碼，可以看到這個功能很簡單，但是被作者實現的很優美。
首先創建類CmdVelSubscribers，該類中包含根據讀入yaml配置文件而構建的一個列表std::vector<std::shared_ptr<CmdVelSubs>> list，以及allowed字段用於記錄當前轉發的的話題idx號。
CmdVelSubs內容為
class CmdVelSubs
{
public:
unsigned int idx; /**< Index; assigned according to the order on YAML file */
std::string name; /**< Descriptive name; must be unique to this subscriber */
std::string topic; /**< The name of the topic */
ros::Subscriber subs; /**< The subscriber itself */
ros::Timer timer; /**< No incoming messages timeout */
double timeout; /**< Timer's timeout, in seconds */
unsigned int priority; /**< UNIQUE integer from 0 (lowest priority) to MAX_INT */
std::string short_desc; /**< Short description (optional) */
bool active; /**< Whether this source is active */
CmdVelSubs(unsigned int idx) : idx(idx), active(false) { };
~CmdVelSubs() { }
/** Fill attributes with a YAML node content */
void operator << (const YAML::Node& node);
};
每一組輸入話題都配有唯一的idx，並記錄了所有我們在的yaml中配置的信息，另外還多了兩個字段，分別為timer計時器，用於記錄輸入時間，以及active記錄當前是否激活的布爾量。
程序初始化時，添加各個話題的接收函數，並給每個話題都初始化了定時器,並初始化了一個全局定時器common_timer，用於對話題未接收超時的情況進行處理。
在接收到速度輸入信息后觸發回調函數：
void CmdVelMuxNodelet::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx)
{
// Reset general timer
common_timer.stop();
common_timer.start();
// Reset timer for this source
cmd_vel_subs[idx]->timer.stop();
cmd_vel_subs[idx]->timer.start();
cmd_vel_subs[idx]->active = true; // 設置idx對應話題為激活
//如果當前沒有被轉發的話題
//或者當前被轉發的話題就是idx對應話題
//或者idx對應話題的優先級高於當前被轉發的話題
//則設置當前轉發話題為idx對應話題
if ((cmd_vel_subs.allowed == VACANT) ||
(cmd_vel_subs.allowed == idx) ||
(cmd_vel_subs[idx]->priority > cmd_vel_subs[cmd_vel_subs.allowed]->priority))
{
if (cmd_vel_subs.allowed != idx)
{
cmd_vel_subs.allowed = idx;
// Notify the world that a new cmd_vel source took the control
std_msgs::StringPtr acv_msg(new std_msgs::String);
acv_msg->data = cmd_vel_subs[idx]->name;
active_subscriber.publish(acv_msg);
}
output_topic_pub.publish(msg);
}
}
重置當前對應的計時器以及全局計時器，並將該話題設置為激活， 如果當前沒有被轉發的話題、或者當前被轉發的話題就是idx對應話題、或者idx對應話題的優先級高於當前被轉發的話題，則設置當前轉發話題為idx對應話題。如果發生了話題變更則發送一次/active消息。
void CmdVelMuxNodelet::timerCallback(const ros::TimerEvent& event, unsigned int idx)
{
if (cmd_vel_subs.allowed == idx || (idx == GLOBAL_TIMER && cmd_vel_subs.allowed != VACANT))
{
// No cmd_vel messages timeout happened to currently active source, so...
cmd_vel_subs.allowed = VACANT;
// ...notify the world that nobody is publishing on cmd_vel; its vacant
std_msgs::StringPtr acv_msg(new std_msgs::String);
acv_msg->data = "idle";
active_subscriber.publish(acv_msg);
}
if (idx != GLOBAL_TIMER)
cmd_vel_subs[idx]->active = false;
}
-
如果觸發了時間回調函數說明當前話題已經超時沒有接收到，則需要把當前的轉發話題設置為空，並將對應輸入話題標記為未激活。
-
值得一提的是，yocs_cmd_vel_mux包采用了nodelet插件的方式編寫，nodelet包可以實現在同一個進程內同時運行多種算法，且算法之間通過shared_ptr通信實現零拷貝，從而減少多個node通過rostcp通信帶來的延時問題。不過這種優勢只有在我們的輸入話題也采用同樣的nodelet方式編寫才能體現出來。
參考：