首页 资讯 应用 高压 设计 行业 低压 电路图 关于

机器人

旗下栏目: 物联网 机器人 新技术 仪器仪表

ROS探索总结—twist_mux多路切换器

机器人 | 发布时间:2018-05-17 | 人气: | #评论# | 本文关键字:ROS,twist_mux,多路切换器
摘要:机器人ROS开发时,常常会遇到相同速度控制消息的选择问题。比如说控制机器人移动的 geometry_msgs::Twist 消息,可以使用键盘节点发布,可以使用手柄节点发布,也可以在导航过程中由movebase发

机器人ROS开发时,常常会遇到相同速度控制消息的选择问题。比如说控制机器人移动的geometry_msgs::Twist消息,可以使用键盘节点发布,可以使用手柄节点发布,也可以在导航过程中由movebase发布,那么这些节点同时运行时,多个节点发布相同的速度控制话题,这个时候机器人就迷茫了,到底该听谁的呢?只能收到哪个数据就听谁的。

针对这样的问题,ROS提供了一种mux多路切换器——twist_mux,可以帮助我们切换到希望接收的数据源上。

 一、原理

正如我们开篇描述的场景,当机器人接收到不同节点发布的速度控制消息时,该如何进行选择呢?

回想一下RTOS中的任务调度原则,每个任务都有自己的优先级,当有多个任务进入等待状态时,系统会选择优先级最高的任务执行,很多系统还支持同等优先级的任务,此时系统会按照时间片轮询的方式执行任务,也就是不偏不倚,平等对待相同优先级的任务。

twist_mux的原理类似于RTOS中的任务调度,需要为每个输入的话题设置优先级,不同话题也可以设置为同样的优先级,但是并不推荐这样做。除了优先级机制外,twist_mux还可以根据超时限制、外部锁话题(lock topics)来限制输入话题的选择。

twist_mux功能包中的核心节点就是twist_mux,其输入输出如下:

image

左侧输入的是多个geometry_msgs::Twist类型的话题,通过twist_mux的选择后,输出唯一的geometry_msgs::Twist话题。下方输入的话题就是用户动态配置选择机制的锁话题,话题的消息类型是Bool,正如锁一样,只有打开和关闭两种状态。这里锁的概念可以理解为:通过限制不同优先级的输入源,达到控制输出的效果。

 

二、配置

twist_mux功能包的安装一句话就可以搞定:

  1. sudo apt-get install ros-indigo-twist-mux

然后就可以运行多路切换器了:

  1. roslaunch twist_mux twist_mux.launch

打印当前的话题列表可以看到:

image

回头再来看一下twist_mux.launch文件里边到底干了啥:

  1. <launch>
  2.   <arg name="joy_vel_in"  default="input_joy/cmd_vel"/>
  3.   <arg name="joy_vel_out" default="joy_vel"/>
  4.   <arg name="cmd_vel_out" default="twist_mux/cmd_vel"/>
  5.   <arg name="config_locks"  default="$(find twist_mux)/config/twist_mux_locks.yaml"/>
  6.   <arg name="config_topics" default="$(find twist_mux)/config/twist_mux_topics.yaml"/>
  7.   <arg name="config_joy" default="$(find twist_mux)/config/joystick.yaml"/>
  8.   <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
  9.     <remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/>
  10.     <rosparam file="$(arg config_locks)"  command="load"/>
  11.     <rosparam file="$(arg config_topics)" command="load"/>
  12.   </node>
  13.   <node pkg="twist_mux" type="twist_marker" name="twist_marker">
  14.     <remap from="twist"  to="$(arg cmd_vel_out)"/>
  15.     <remap from="marker" to="twist_marker"/>
  16.   </node>
  17.   <node pkg="twist_mux" type="joystick_relay.py" name="joystick_relay" output="screen">
  18.     <remap from="joy_vel_in"  to="$(arg joy_vel_in)"/>
  19.     <remap from="joy_vel_out" to="$(arg joy_vel_out)"/>
  20.     <rosparam file="$(arg config_joy)" command="load"/>
  21.   </node>
  22. </launch>

可以看到,在launch文件中配置了一些话题名参数,在启动twist_mux节点的过程中还加载了两个配置文件,twist_mux_topics.yaml对应的就是输入话题的配置,twist_mux_locks.yaml对应的是锁话题的配置。

(1)twist_mux_topics.yaml

  1. # Input topics handled/muxed.
  2. # For each topic:
  3. # - name    : name identifier to select the topic
  4. # - topic   : input topic of geometry_msgs::Twist type
  5. # - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead
  6. # - priority: priority in the range [0, 255]; the higher the more priority over other topics
  7. topics:
  8. -
  9.   name    : navigation
  10.   topic   : nav_vel
  11.   timeout : 0.5
  12.   priority: 10
  13. -
  14.   name    : joystick
  15.   topic   : joy_vel
  16.   timeout : 0.5
  17.   priority: 100
  18. -
  19.   name    : keyboard
  20.   topic   : key_vel
  21.   timeout : 0.5
  22.   priority: 90
  23. -
  24.   name    : tablet
  25.   topic   : tab_vel
  26.   timeout : 0.5
  27.   priority: 100

该配置文件中包含了一个输入话题的配置列表,每个输入话题的配置项包含以下几个:

  • name:一个用户可读的命名,不是话题名,只用于调试显示;

  • topic:话题名,话题必须是geometry_msgs::Twist类型;

  • timeout:消息允许的超时限制,超过此时间仍然没有数据后,会切换到其他输入话题上,如果设置为0的话,相当于没有限制,会无限等待;

  • priority:输入话题的优先级,0~255之间,值越大优先级越高

 

(2)twist_mux_locks.yaml


  1. # Locks to stop the twist inputs.
  2. # For each lock:
  3. # - topic   : input topic that provides the lock; it must be of type std_msgs::Bool?!!!
  4. # - timeout : == 0.0 -> not used
  5. #              > 0.0 -> the lock is supposed to published at a certain frequency in order
  6. #                       to detect that the publisher is alive; the timeout in seconds allows
  7. #                       to detect that, and if the publisher dies we will enable the lock
  8. # - priority: priority in the range [0, 255], so all the topics with priority lower than it
  9. #             will be stopped/disabled
  10. locks:
  11. -
  12.   name    : pause
  13.   topic   : pause_navigation
  14.   timeout : 0.0
  15.   # Same priority as joystick control, so it'll not block it.
  16.   priority: 100
  17. -
  18.   name    : loop_closure
  19.   topic   : stop_closing_loop
  20.   timeout : 0.0
  21.   priority: 200
  22. -
  23.   name    : joystick
  24.   topic   : joy_priority
  25.   timeout : 0.0
  26.   priority: 100

关于锁话题的配置也是一个列表,每一个子项都是一个锁话题的配置,配置项包含以下几个:

  • name:一个用户可读的命名,不是话题名,只用于调试显示;

  • topic:锁话题名,消息类型必须是std_msgs::Bool 类型;

  • timeout:锁话题需要按周期发布,所以超过时间限制后,会认为发布锁的节点掉线了,锁失效,类似于看门狗的功能;如果设置为0的话,则没有超时限制,持续有效;

  • priority:限制输入话题的优先级,0~255之间,在此值以下优先级的话题,会被上锁,开锁之前无法输出,以达到动态控制输出的效果。

 

三、实践

现在我们就来通过时间验证一下twist_mux的效果。

首先启动twist_mux节点:

  1. roslaunch twist_mux twist_mux.launch

然后监听最终输出的速度控制消息:

  1. rostopic echo /twist_mux/cmd_vel

1. 多输入源的选择

目前还没有任何输入,所以还不会有输出的消息。

接着我们就可以发布几个输入的消息了,先发布一个导航输出的速度消息:


  1. rostopic pub -r 10 /nav_vel geometry_msgs/Twist "linear:
  2.   x: 1.0
  3.   y: 0.0
  4.   z: 0.0
  5. angular:
  6.   x: 0.0
  7.   y: 0.0
  8.   z: 0.0"

成功发布之后,输出监听并没有任何输出,这是因为在锁消息中,我们设置的最小优先级是100,而nav_vel的优先级在配置文件中设置的是10,自然被挡到了门外,没办法输出。

再来发布joy_vel消息:

  1. rostopic pub -r 10 /joy_vel geometry_msgs/Twist "linear:
  2.   x: 2.0
  3.   y: 0.0
  4.   z: 0.0
  5. angular:
  6.   x: 0.0
  7.   y: 0.0
  8.   z: 0.0"

很快就可以在输出监听的终端中看到如下图所示的joy_vel的消息了:

image

此时有nav_vel和joy_vel两个输入消息,经过twist_mux的选择后,只输出了joy_vel消息。

那么如果我们想要输出nav_vel怎么办呢?

在nav_vel和joy_vel两个消息保持发布的状态下,重新打开一个窗口,关闭优先级门槛的限制:

  1. rostopic pub /joy_priority std_msgs/Bool "data: false"

此时如果joy_vel消息停止或者超时的话,twist_mux就会自动切换到nav_vel输出,我们可以直接kiil掉joy_vel的话题发布,输出监听的终端就会显示nav_vel消息的数据了:

image

2. 停止某优先级以下的输入源

如果我们先要暂停输出,怎么办呢?锁消息中也设置了暂停nav_vel的话题:

  1. rostopic pub /pause_navigation std_msgs/Bool "data: true"

现在nav_vel话题就暂停了,想要恢复的话,只需要在发布一次pause_navigation消息即可:

  1. rostopic pub /pause_navigation std_msgs/Bool "data: false"

需要注意的是,/pause_navigation话题设置的优先级和joy_priority是一样的,所以这里的暂停无法暂停优先级100及以上的话题,所以并不会影响joy_vel消息的转发。

我们还在锁配置中设置了一个优先级为200的控制锁,一旦我们生效这个锁,优先级200以下的话题都会停止转发:

  1. rostopic pub /stop_closing_loop std_msgs/Bool "data: true"

现在所有话题都无法通过twist_mux输出了。

OK,现在我们应该已经明白了twist_mux功能包的原理和使用方法,灵活使用锁配置,可以让我们轻松控制多个输入源的切换。


责任编辑:电气自动化网
首页 | 电气资讯 | 应用技术 | 高压电器 | 电气设计 | 行业应用 | 低压电器 | 电路图 | 关于我们 | 版权声明

Copyright 2017-2018 电气自动化网 版权所有 辽ICP备17010593号-1

电脑版 | 移动版 原创声明:本站大部分内容为原创,转载请注明电气自动化网转载;部分内容来源网络,如侵犯您的权益请发送邮件到[email protected]联系我们删除。