ROS中那些好用的功能包推荐（1）—— yocs_cmd_vel_mux多路复用速度控制_人工智能

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

一、 yocs_cmd_vel_mux是怎么工作的

我们需要在配置文件中对该功能包接收的话题和优先级进行配置，它可以接收多个话题并将其基于优先级发送，处理流程示意如下：

ROS中那些好用的功能包推荐（1）—— yocs_cmd_vel_mux多路复用速度控制
需要注意的是，只要高优先级话题当前有发布就会覆盖低优先级的话题，因此可以通过让高优先级信息源持续发送速度0的信息来控制移动机器人停下。

1.1 接收的话题

~input/cmd_vel：即为要处理的速度话题，话题格式为(geometry_msgs/Twist),接收的话题名称可以自己配置。

1.2 发布的话题

~output/cmd_vel：即为发送给控制器的速度话题，话题格式为(geometry_msgs/Twist),话题名称可以自己配置。
~active：即为当前激活的输入话题所对应配置的name，话题格式为(std_msgs/String),话题名称在代码中固定为/active。

二、安装与配置过程

2.1 安装

git地址： https://github.com/yujinrobot/yujin_ocs.git
注意需要根据你的ROS版本选择对应的分支：

melodic: release/0.8-melodic
kinetic: kinetic
下载后catkin_make编译yocs_cmd_vel_mux功能包，并去param文件夹中参考example.yaml去配置自己的映射。
给大家一个博主的配置作为参考：

subscribers:
  - name:        "Navigation control"
    topic:       "movebase/cmd_vel/navigation"
    timeout:     0.5
    priority:    1
    short_desc:  "Navigation control"

  - name:        "Remote control"
    topic:       "movebase/cmd_vel/remote"
    timeout:     0.1
    priority:    2

  - name:        "Application control"
    topic:       "movebase/cmd_vel/webapp"
    timeout:     0.3
    priority:    3

  - name:        "Keyboard operation"
    topic:       "movebase/cmd_vel/keyop"
    timeout:     0.1
    priority:    4

  - name:        "Joystick control"
    topic:       "movebase/cmd_vel/joystick"
    timeout:     0.1
    priority:    10
    short_desc:  "Joystick control"

publisher:       "movebase/cmd_vel"

可以看到之前的每组输入都可以几个参数，分别为：
name: 数据源名称
topic: 提供cmd_vel的话题名称
timeout:某话题超过该时间没有新的输入则认为该输入话题不活跃（单位：s）。
priority:优先级，范围为（0-MAX_INT）数字越大优先级越高。
short_dessc：简短描述，可以不填。

最后一行的publisher字段为配置的发布话题。

我们可以把自己的配置文件任意命名，放置于此文件夹中。

2.2 运行

进入到launch文件夹中配置launch文件，参考standalone.launch文件进行配置，注意要修改config_file配置文件的地址。

三、扒一下代码实现

查看了一下源代码，可以看到这个功能很简单，但是被作者实现的很优美。
首先创建类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方式编写才能体现出来。