标签：subs idx cmd mux 话题 vel yocs
-
在我们控制一个移动机器人运动时，可能会遇到如下场景：自研的移动机器人在自动导航的过程中突然迷路要撞墙了，一场车祸马上就要发生，这时候，我们就会很希望能够通过无线手柄或者键盘去控制小车紧急停车，让小车改邪归正，迷途知返。想要实现这个功能，就需要用到多路输入复用控制，即把多种速度控制信号收集起来，并按照优先级发给小车，覆盖掉自主导航的速度控制消息。
-
幸运的是，完善的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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。