讓機器人平滑移動以及增加應用彈性的方法

有沒有一個經驗,為什麼我做的機器人移動上永遠無法像 PR2 或者吸塵器機器人一樣平滑?為什麼我的機器人每次移動都好像一瞬間花了洪荒之力,而機身就是被不情願的往前脫去,這樣久了不只對馬達是種損害,對電力系統也宣判了慢性癌症,總有一天會讓電力系統中某個元件壞掉(如果你的電力系統跟我設計的一樣永遠都不完美)。

 

因此,我們需要為自己的導航系統加點控制系統,讓機器人移動上不會這麼魯莽。我們使用 Yujin robot (他們的 GitHub) 開發的 kobuki_safety_controllerkobuki_velocity_smoother 。其實這兩個 packages 是包在yujin_yocs 這個 stack 中。詳細的官方資訊可以參考這裡。有一點稍微提下,就是這些 Packages 裡面裝的,並不是節點,而是 nodelets (不知道怎麼翻XDD)。但這之後我們會再聊到,本篇先從略。我們來看一下整個驅動控制架構:

si_Ih7qGws9qsRJwX5xTb_g
圖一:Kobuki 整體控制架構

Source: kobukiTutorialsKobuki’s Control System

 

 

 

由圖一中可以觀察到,Kobuki 這台吸塵器機器人(加上上面的架子後就進化成 Turtlebot了,看倌可以花三萬塊台幣去收服它!)所採用的,就是這一套馬達驅動控制架構。yocs_safety_controller 其實由三個程式組成:kobuki_node 、cmd_vel_mux、kobuki:_safety_controller。其中,kobuki_node 其實就是 kobuki 吸塵器機器人的控制驅動節點。cmd_vel_mux 是一種多進單出的速度切換器。kobuki_safety_controller 則是檢查機器人是否碰撞到障礙物、輪子脫落、有沒有跑到懸崖上的檢查機制。

 

啟動速度切換器!

 

所以,我們可以利用這三個節點中的 cmd_vel_mux 來作切換器,這個節點確認就算有不同的速度指令同時要操控這台機器人,只有一種速度指令會傳送給馬達。它有10個 /cmd_vel 的接口,可以接上十種速度指令,並指定每個速度指令的優先性先後順序,由最優先的10到最後一位的第0位。預設是讓 ROS Nav stack 的優先性設在最後一位,第零位,而讓 safety controller 輸出的速度指令設在第十位,使用者遙控輸出的指令排在前述兩者之間。這樣子,多進單出的 cmd_vel_mux 切換器就完善了。另外,這個切換器還會輸出一個總輸出速度 /cmd_vre_mux/output 作為閉路回饋控制迴圈的輸入依據。

這樣講有點抽象,讓我舉個例子:

優先性:

10 . (highest priority) safety controller

  1.   keyboard teleop
  2.   joystick teleop
  3.   android teleop
  4.  (Lowest priority) nav cmd vel

 

以這個方式,我們一次接上了5個速度指令,而一次只有一個總輸出給馬達控制器。但在輸出到馬達之前,我們要在加上一個控制器。

 

那要如何實做呢?既然我們都有節點了,那麼現在要擔心的,是怎麼用 Launch file 來開啟每個節點以及輸出的 topics。其實不只是要學會用 launch,具體的速度指令優先順序,必須另外寫個社檔宣告,然後匯入到 launch 檔。範例如下:

 

<launch>
 <arg name="nodelet_manager_name" default="nodelet_manager" />
 <!–Velocity Smoother’s settings–>  <arg name="smoother_node_name"    default="velocity_smoother" />

 <arg name="config_file"           default="$(find mybot_bringup)/launch/velocity_smoother_param.yaml"/>

 <arg name="raw_cmd_vel_topic"     default="cmd_vel_mux/output"/>                <!–subscribed topic –>

 <arg name="smooth_cmd_vel_topic"  default="/smooth_vel"/>                          <!–publish topic–>

 <arg name="robot_cmd_vel_topic"   default="cmd_vel_mux/output"/>               <!–subscribed topic–>

 <arg name="odom_topic"            default="odom"/>                                             <!–subscribed topic–>
 <!– ****** Nodelet manager ******** –>

 <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" />
 <!– ***** cmd_vel_mux ************* –>

 <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet $(arg nodelet_manager_name)">

    <param name="yaml_cfg_file" value="$(find mybot_bringup)/launch/cmd_vel_mux_minimal_with_safety.yaml" />

 </node>

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>
   <!– velocity commands I/O –>

   <remap from="$(arg smoother_node_name)/raw_cmd_vel"        to="$(arg raw_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/smooth_cmd_vel"  to="$(arg smooth_cmd_vel_topic)"/>
   <!– Robot velocity feedbacks –>

   <remap from="$(arg smoother_node_name)/robot_cmd_vel"  to="$(arg robot_cmd_vel_topic)"/>

   <remap from="$(arg smoother_node_name)/odometry"          to="$(arg odom_topic)"/>

 </node>

</launch>

 

接著我們來看一下設定檔,範例如下,:

 

cmd_vel_mux_with_safety_priority.yaml

subscribers:

 – name:        “Default input"

   topic:       “def_cmd_vel"

   timeout:     0.1

   priority:    0

   short_desc:  “Default velocity topic; controllers unaware that we are multiplexing cmd_vel will come here"
 – name:        “Navigation stack"

   topic:       “nav_cmd_vel"

   timeout:     0.5

   priority:    1

   short_desc:  “ROS navigation stack controller"
 – name:        “Safety Controller"

   topic:       “cmd_vel_safety"

   timeout:     0.2

   priority:    10

   short_desc:  “Kobuki’s safety controller"
 – name:        “Keyboard operation"

   topic:       “key_cmd_vel"

   timeout:     0.1

   priority:    9
 – name:        “Remote control"

   topic:       “rem_cmd_vel"

   timeout:     0.1

   priority:    8
 – name:        “Onboard joystick"

   topic:       “joy_cmd_vel"

   timeout:     0.1

   priority:    7
 – name:        “Web application"

   topic:       “web_cmd_vel"

   timeout:     0.3

   priority:    6

 

平滑輸出速率!

 

儘管 ros_control 架構下的控制程式本身就有速度平滑化的特點。但萬一我們使用的平台並非遵守這個架構,或者只想雙倍確認速度控制不會出錯,那麼我們可以使用 Yujin Robot 「出品」的 yocs_velocity_smoother 。方法很簡單,把 cmd_vel_mux 輸出的速度指令直接接到 yocs_velocity_smoother 的入口,這樣子,以目前 Yujin 給的 PID 參數,應該就可以應付您的機器人控制了。

 

從官網的ROS Wiki網頁上可以看得出來,Velocity Smoother Nodelet 的輸入與輸出:

輸入:

~raw_cmd_vel (geometry_msgs/Twist)

~odometry (nav_msgs/Odometry)

~robot_cmd_vel (geometry_msgs/Twist)

 

其中,如果沒有額外付上其他閉路控制設計,那麼 raw_cmd_vel 的輸入 topic 可以和輸入 robot_cmd_vel 的一樣。

 

輸出:

~smooth_cmd_vel (geometry_msgs/Twist):即平滑化後的速度指令,直接給 base_controller 節點。

此外,對於網頁上提及的參數,也可以另外寫成一個設定檔,由 launch 載入,讓其自動存進 parameter server 裡面。範例如下:

 

# Example configuration:

# – velocity limits are around a 10% above the physical limits

# – acceleration limits are just low enough to avoid jerking
# Mandatory parameters

speed_lim_v: 0.8

speed_lim_w: 5.4
accel_lim_v: 0.3

accel_lim_w: 3.5
# Optional parameters

frequency: 20.0

decel_factor: 1.0
# Robot velocity feedback type:

#  0 – none

#  1 – odometry

#  2 – end robot commands

robot_feedback: 2

 

在 launch 檔載入的方式其實已經出現在上述的範例裡面,我們節錄出來:

 

  <!– ****Velocity Smoother (just regulate the overall output from cmd_vel_mux) ***** –>

 <node pkg="nodelet" type="nodelet" name="$(arg smoother_node_name)"

       args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager_name)">

   <!– parameters –>

   <rosparam file="$(arg config_file)" command="load"/>

 </node>

 

好了,如果把所有的節點搞定,輸出與輸入或對接的 Topics 都有接對(我還真的自己拿筆出來畫圖!),那麼,現在就可以打開你的機器人,遠端遙控一下!光是只給一個 Twist 都可以發現機器人的移動行為變了!這就表示,你成功了!

 

如何讓一台機台中的 Launch 檔也能啟動其他機台中的節點?

distributed-computing
之前看了林信男的這篇如何用一個 launch 檔啟動期胎台機器人上的節點(程式),看得我也心癢癢的,想說這樣的功能,真是 ROS 界的福音啊!趁著研究告一段落,比較有空的時候,也來試試。以下就是我的使用心得!
在開始之前,請先確定你已經完成了 DSA 無密碼遠端連線的設定,請參考這裡。如果還沒設定或還沒閱讀過,請連過去看。對!就是現在!我在這邊等著(啜飲焦糖瑪奇朵中)。
OK了嗎?好,確定一下你的 ~/.bashrc 裡面已經設定好 ROS Master 地址、你自己的 workspace 位置。都設定好了嗎?那我們繼續。
Launch 檔中有幾款標籤可以自由使用,請參考 這篇官方Launch/XML machine 標籤的用法教學 。雖然裡面有提供 password 選項,但既然我們能不須密碼遠端登入,那洩漏密碼這件事遍布成問題了。
ROS 的 Launch 檔裡,允許使用者針對各機台上的節點做啟動,但不可以遠端啟動另外一個 Launch 檔。此外,當初的設計應該是讓 Master 端用這樣的「至尊 Launch 檔」呼風喚雨,所以如果你是在非 master 的機台上呼叫這樣的 Launch 檔,你只會開啟這台基台上的節點而已。這是使用尚要注意的地方。在撰寫這類至尊 Launch 檔的語法上,我們需要用 <machine>標籤先宣告各機台,然後啟動各節點時,用 <node>裡面的 machine="" 來指定機台。
以下是一個範例檔。有很多標籤,既然是 Optional ,我就不使用了:
 ———————————————————————-
<launch>
<!–
This launch file activates both the robot and client side launches
going across all machines. Before using, please verify that all machines
are connected under the same network.
Make sure to set up DSA or password-less login to avoid compromising
your own password. Better if you can set DSA on all computers.
CAUTION: ONLY LAUNCH THIS FILE AT THE MASTER side, otherwise it won’t work.
–>
<arg name="client_ip" default="charly-MSI.local" />
<arg name="client_name" default="charly" />
  <!–The robot–>
  <machine name="myRobot"
  address="myRobot.local"
  user="bot" />
  <!–The client–>
  <machine name="client_pc"
  address="$(arg client_ip)"
  user="$(arg client_name)" />
  <!–機器人端開啟雷射測距儀–>
  <node machine="myRobot" pkg="hokuyo_node" type="hokuyo_node" name="hokuyo_node">
      <param name="frame_id" type="string" value="hokuyo_link" />
  </node>
  <!–客戶端開啟 Rviz–>
  <node machine="client_pc" name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

——————————————————————

自己使用的心得

雖然期盼已久的遠端單 launch file 開啟節點的夢終於實現了,但是我也碰到另外兩個問題,

問題一

所有的文件都解釋了如何啟動遠端節點的實作方法,但是竟然沒有超出四篇在談論如何遠端啟動對方的 launch 檔,亦言之,無法用 machine 標籤將別台機器的 launch 檔包進來。對我而言,目前我手上的專案根本無法一個節點一個節點置入 machine 標籤,因為一個專案就包括了數十個節點,改完手會斷。因次,我個人的使用經驗是挫折的,目前好像尚未能找到這樣的實作方法,看起來又必須重新回覆到之前遠端到各機台並個別啟動該機的 launch file 模式了,否則根本無法順利運作。

問題二

若你在非 master 節點所在的機台上啟動這個可控遠端的 launch 檔,你會遭遇到一則錯誤的訊息,就是 ROS 找不到 master, 所以無法啟動節點,laumch 就此關閉。這就表示,你只能在 master 的機台端啟動該 laumch 檔。
如果你的 master 剛好就在你現在所使用的電腦上,那就沒問題了,但如果你跟我一樣,master 放在機器人端,那麼你依舊每次還要遠端過去,然後才執行該 launch 檔,用久了自己都會嫌麻煩。那你可能會問,那為什麼不乾脆直接把 master 設在自己的電腦上就好?Well, 這世界上永遠都有特例,例如,當你所使用的機器人是公用的,隨時都會有好幾個人一起連到機器人的電腦上。除非今天你是老闆,否則應該沒有人敢一直請你開電腦,然後才遠端到你的電腦上,只為了啟動他的程式吧?
所以,主要的問題是 master 節點,這也可能是為什麼 ROS 2.0 可以改進的,就是去除必須啟動 master 節點才能做事的奇怪要求。

每次登入都要輸入密碼?用 DSA 實作無密碼遠端登入!

由於最近必須把所有的研究成果整合到一台全新的機器人平台上,所以有機會來嘗試一些之前沒時間嘗試的工具。譬如說只用一個 launch 檔同時開啟多台機器上的各個節點,這樣勢必會讓現在啟動機器人的方法發生革命性的變化,至少應該會更簡單。但在這之前,我想分享一下怎麼設定不需要密碼登入的遠端連線這回事,有這個前置動作,才會有更好的連線控制方法。好,那我們來實作看看!

Step 1  至少先確認能遠端

首先,我們需要確認兩台電腦能彼此遠端。請參考我的另一篇文章
在這邊只提醒各位,機器人端和客戶端個別都需要安裝 openssh-client 和 openssh-robot
$ sudo apt-get install openssh-client
$ sudo apt-get install opensh- server
直白來說,前者是遠端到別人那端時使用的,後者則是讓自己當伺服器或接受別人連到我們這邊。
如果不確定自己有什麼裝了什麼沒裝,可以安裝並使用 aptitude 這個小工具幫忙搜尋(如果你的系統中沒有。直接 apt- get install 它),用 Aptitude 搜尋方法如下:
$ aptitude search openssh*
在這一步快進入尾聲前,請確認自己的 .bashrc 內已經有以下:
export ROS-HOSTNAME=你的 Zeroconf 名稱
export ROS_MASTER_URI=http://[Master 的 Zeroconf 名稱]:11311/
例如我的設定如下:
export ROS_HOSTNAME=charly-MSI.local
export ROS_MASTER_URI=http://beebot.local:11311/
只要輸出的所有程式那行的前面出現 “i", 那麼就代表該程式已經安裝。

Step 2 打造金鑰

在機器人的終端機上打:
$ ssh -keygen -t dsa
這時終端機應該會跳出類似以下的訊息:
Enter file in which to save the key (/home/[機器人 host]/.ssh/id_dsa)
它已經有預設的儲存位置了, 就給把公鑰直接儲存在該路徑上吧!我們之後會需要它, 所以這一步就給它大方 Enter 鍵揍下去(噹噹噹噹!)
終端機會在輸出一行訊息:Enter passphrase:
就輸入密碼!
Enter same passphrase again: 再輸入密碼 Orz
所以目前公鑰存在以下路徑:/home/[我的機器人]/.ssh/id_rsa.pub
還有自己的私鑰, 這個永遠別給別人, 記得喔! /home/[我的機器人]/.ssh/id_rsa

Step 3 給對方公用金鑰

是的。你的家要讓朋友隨時進出,那你就要給朋友你的鑰匙。但這份鑰匙為了門禁驗證用,你只把半個鑰匙給人家,自己保留另外一半,或者用另外一種方式看,有點像你自己這邊設置門鎖。若其他朋友冒充你的朋友拿不知哪來的一半鑰匙開門,那對不起,依舊無法打開你家的門。
所以,再電腦遠端驗證中,你自己持有一把私鑰,給想要連近來的電腦一把公鑰。那要怎麼給呢?首先,先改公鑰(.ssh)執行權限:
$ cd
$ chmod 755 .ssh
複製公鑰到要遠端連進來的電腦上:
$ scp ~/.ssh/id_rsa.pub [客戶端電腦]@[客戶端電腦的 IP 或 Zeroconf]: .ssh/authorized_key
例如:
$ scp ~/.ssh/id_rsa.pub charly@charly-MSI:.ssh/authorized_key
現在,遠端到你的客戶端電腦上,打開終端機後,設定公鑰權限:
$ chmod 600 ~/ssh/authorized_keys
好了!設定完成!

Step 4 最後一次需要密碼的登入,此後一勞永逸

假設你從你的客戶端電腦用 ssh 遠端連線到機器人主機,例如:
$ ssh bot@myRobot.local
系統會問你最後一次輸入對方的密碼,一旦輸入好並登入後,之後便在也不會要求你輸入密碼。你已經可以順利以不輸入密碼的方式,登入對方的電腦。同樣的,若對方要遠端連線登入到你這台電腦,也必須照上述設定步驟,只是這次轉換角色而已。

Step 5 (Optional) 寫一個批次檔讓你每次都能輕鬆登入

科技始於人性。我不想每次都一直打 ssh 指令,有沒有更快的方式,讓登入便得更便捷自動?有的!批次檔現在可以更安全的幫你再不洩漏密碼的強況下,自動遠端登入。先在你中意的路徑下,建立一個文件檔,隨便取什麼名字,副檔名為 .sh,裡面打下遠端登入的指令即可。以下範例是我的遠端批次檔:
#! /bin/sh
ssh bot@myRobot.local
就這麼簡單!