使用yocs_cmd_vel_mux進行機器人速度控制切換

　　cmd_vel_mux包從名字就可以推測出它的用途，即進行速度的選擇（In electronics, a multiplexer or mux is a device that selects one of several analog or digital input signals and forwards the selected input into a single line）。這一功能有時候很有用，比如機器人在自主導航時遇到緊急情況要切換到手動控制模式，這時就要將機器人控制權從自主導航的節點切換到方向盤、手柄、鍵盤、急停開關等控制節點。

　　如果有多個節點要想控制機器人的運動，多路復用器(cmd_vel_mux)允許平行使用所有這些命令，並且可以設置優先級，優先級高的會搶占優先級低的任務。例如機器人自主導航時可能會有下面4種優先級的任務（通常將自主行為的優先級設為最低）：

• 3 (highest priority): safety controller （安全控制）
• 2: keyboard teleop （鍵盤遙控）
• 1: android teleop （Android 遙控）
• 0 (lowest priority): navi stack teleop （導航包自主控制）

　　在一般情況下，大部分時間都是navi包控制機器人。但是通過Android遙控app或鍵盤遙控觸發命令，可以覆蓋navi包的命令。如果兩個都發出命令，那么會優先使用鍵盤的遙控命令。當碰撞、跌落傳感器發出報警信號，觸發safety controller的命令時，則又會重寫上面所有的速度命令，這樣就能保證機器人的自主移動時的安全。

　　下載yocs_cmd_vel_mux包的源文件到ROS工作目錄的源文件夾中（比如catkin_ws/src），然后使用catkin進行編譯。

　　先看一下包中的standalone.launch文件。先運行一個名為nodelet_manager的nodelet manager節點，然后加載cmd_vel_mux.launch文件：

<!--
  Example standalone launcher for the velocity multiplexer.
  
  For best results you would usually load this into the core control system
  to avoid latency for reactive controllers. However for other apps, running
  it standalone with a similar configuration will also work.
 -->
<launch>
  <arg name="nodelet_manager_name"  value="nodelet_manager"/>
  <arg name="config_file"           value="$(find yocs_cmd_vel_mux)/param/example.yaml"/>
  
  <!-- nodelet manager -->
  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>
  
  <!-- velocity mulitplexer -->
  <include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
    <arg name="nodelet_manager_name"  value="$(arg nodelet_manager_name)"/>
    <arg name="config_file"           value="$(arg config_file)"/>
  </include>
</launch>

　　在cmd_vel_mux.launch中使用nodelet_manager加載yocs_cmd_vel_mux/CmdVelMuxNodelet

<!--
  YOCS Velocity Multiplexer launcher
  -->

<launch>
  <arg name="nodelet_manager_name"  default="nodelet_manager"/>
  <arg name="config_file"           default="$(find yocs_cmd_vel_mux)/param/example.yaml"/>

  <node pkg="nodelet" type="nodelet" name="yocs_cmd_vel_mux"
        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg nodelet_manager_name)">
    <param name="yaml_cfg_file" value="$(arg config_file)"/>
  </node>
</launch>

　　下面是修改的配置文件，有默認、手柄和鍵盤3種優先級不同的控制方式。優先級從0到MAX_INT，數字越大優先級越高：

# Individual subscriber configuration: # name: Source name # topic: The topic that provides cmd_vel messages # timeout: Time in seconds without incoming messages to consider this topic inactive # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT # short_desc: Short description (optional) 
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"

 

　　下面以ROS中的小海龜為例子來展示一下cmd_vel_mux的使用方法。編寫一個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也變為鍵盤控制的速度：

 

參考：

yocs_cmd_vel_mux

Kobuki's Control System

ROS nodelet的使用

Kobuki入門教程-Kobuki控制系統