• 在我们控制一个移动机器人运动时，可能会遇到如下场景：自研的移动机器人在自动导航的过程中突然迷路要撞墙了，一场车祸马上就要发生，这时候，我们就会很希望能够通过无线手柄或者键盘去控制小车紧急停车，让小车改邪归正，迷途知返。想要实现这个功能，就需要用到多路输入复用控制，即把多种速度控制信号收集起来，并按照优先级发给小车，覆盖掉自主导航的速度控制消息。

• 幸运的是，完善的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方式编写才能体现出来。

参考：

使用yocs_cmd_vel_mux进行机器人速度控制切换

古月居: ROS中那些好用的功能包推荐（1）—— yocs_cmd_vel_mux多路复用速度控制