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方式编写才能体现出来。
参考：
古月居: ROS中那些好用的功能包推荐（1）—— yocs_cmd_vel_mux多路复用速度控制
标签：subs,idx,cmd,mux,话题,vel,yocs 来源： https://www.cnblogs.com/chenweihan/p/15096895.html