ROS in Self-driving Car system

這是經歷了漫長的時間, 最後的一哩路了….從2016年12月開始, 到2018年4月中, 花了整整一年五個月. 其實我原先打算半年前就畢業的, 但是中途有狀況, 所以只好 term2 完成後停了半年才開始 term3, 也因此到昨天才剛確定畢業! 而昨天剛好也參加了 Udacity 在中國兩周年的會, 見到了 David Sliver 本人, 算是畢業的一個小紀念!

最後的 project 比較有別於以往, 採用 team work 的方式. 我們的 team 共五人, team lead Franz Pucher 德國, Theodore King 美國, 和我. 疑? 另外兩個呢? 對於 project 完全沒貢獻…我不想說了….= =

ROS 簡介

關於機器人控制和自動車都會使用 ROS (Robot Operating System), ROS 一定要參考 ROS wiki. 本次作業的 ROS 系統擷取課程圖片如下:

看不懂沒關係, 了解 ROS 主要三個概念: Node, Topic, Msg 就清楚上面的圖在幹嘛了. Node 簡單講類似於 class, 可以訂閱某些 Topic, 和發送 Msg 到指定的 Topic. 舉例來說當有某個 Node A 發送一個 msg M 到一個 topic T 時, 如果 Node B 有訂閱 topic T, 則 Node B 會收到 msg M, 並且執行預先設定好的 call back function. 用以下的程式範例舉例:

class TrafficLightDetector(object):
    def __init__(self):
        rospy.init_node('tl_detector')  # 要在開頭就先 init 好這是 ros node
        ...
        # 訂閱了一個 topic '/current_pose', 並且如果有 msg 發送到此 topic, 此 node 會收到並且呼叫 call back function self.pose_cb
        sub = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
        # 此 node 會發送 msg 到 topic '/traffic_waypoint'
        self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
        
    def pose_cb(self, msg):
        self.pose = msg.pose

要注意的是, 由於 topic 運作方式為一旦有其他 node 發送 msg 到此 topic, 有訂閱此 topic 的 node 的 call back function 都會被呼叫. 這就意謂著 topic 如果發送 msg 太頻繁, 導致訂閱的 node 無法及時消化, 則 msg 會掉包. 一種解決方式為使用 rospy.Rate 控制發送的頻率. 但是其實還有另一種傳送 msg 的方式: Service

簡單講 Service 的概念就是 request and response, 不同於 topic, service 會將兩個 node 直接連接起來, 一個發起 request 後, 會等另一個 node response 才會接著做下去. 一個簡單的舉例如下:

# 需注意 ServiceClassName 要先在 package 裡的 srv folder 定義好
class NodeA(object):
...
    # 拿到該 service
    service = rospy.ServiceProxy('service_name',ServiceClassName)
    # 拿到可以 request 的 msg instance
    msg = ServiceClassNameRequest()
    # 修改 msg 成需要的狀態
    ...
    # 發起 request 並得到 response
    response = service(msg)
    
class NodeB(object):
...
    rospy.Service('service_name',ServiceClassName, self.handler_func)
    ...
    def handler_func(self, msg):
        # 收到 request 的 msg, 在此 handler function 負責處理如何 response
        ...

上面的範例使用了兩個 nodes, node A 負責發起 request, 而 node B 負責 response. 另外筆記一些 ros 常用的指令和功能

>> roscore # start ROS master
# rosrun 可以指定負責要跑哪個 node
>> rosrun package_name node_name
# node 一多, 可以使用 roslauch 一次執行多個 nodes, 但是要寫好 launch file
>> roslaunch launch/launchfile
# 列出 active 的 nodes
>> rosnode list
# 列出 active 的 topics
>> rostopic list
# 查看某個 topic
>> rostopic info topic_name
# 將 publish 到此 topic 的 msgs 都即時顯示在 terminal 上
>> rostopic echo topic_name
# 一般來說 rospy.loginfo('info msg') 會顯示在 /rosout 這個 topic, 因此適合 debug
>> rostopic echo /rosout
# 查看某個 msg
>> rosmsg info msg_name
# build 自定義的 ros package
>> cd ~/catkin_ws; catkin_make
# 檢查 package 的 dependency
>> rosdep install -i package_name
# 如果將某個 package 加入到自己的 catkin_ws 時, 需加到 catkin_ws/src 資料夾下, 並且重新 make
>> cd ~/catkin_ws/src
>> git clone 'some packages'
>> cd ~/catkin_ws
>> catkin_make
# Build 完後, 需要 source 才可以將 catkin_ws/src 下的所有 packages 都加到 ros 中
>> source ~/catkin_ws/devel/setup.bash

Debug 的話 rospy.loginfo, rospy.logwarn, rospy.logerr, rospy.logfatal 很好用, 它們分別會被記錄在以下幾個地方:

Self-Driving Car ROS Nodes

因此這最後的 project 主要就分成三個部分

Perception:
這部分負責收到 /image_color 這個 topic 的影像後, 來找出 traffic sign 在哪裡並且是哪種燈號. 相當於 term1 的 Vehicle Tracking, 我主要負責此部分, 但是沒有使用當時做 project 的 sliding window + svm 方法. 下面會詳細介紹.
Planning:
負責根據目前車子的位置以及如果有紅燈的話, 必須規劃好新的路徑, 並將期望的速度一併發送給 Control. 相當於 term3 的 Path Planning
Control:
根據規畫的路徑和速度, 找出可以實際操控的參數 (throttle, brake, steering). 相當於 term2 的 Model Predictive Control. 但我們團隊沒有用 MPC, 而是使用 PID control.

Perception Traffic Light

由於小弟我不是做 CV 的, 沒這麼多厲害的能力, 因此一開始我也沒打算訓個 YOLO 之類的方法. 重頭開始訓練的話我只能先想到不如用上次 project 的 semantic segmantation 方法, 將認為是 traffic sign 的部分找出來, 接著用簡單的顏色區分一下好了.

training set 我使用 Bosch Traffic Light Dataset, 共有 5093 張 images. 很多張影像完全沒有 traffic sign, 因此我就忽略, 並且有些 traffic sign 實在太小, 那種情況也忽略, 最後篩選出 548 張有 traffic signs 的影像並且 resize 成 600x800, 舉個例如下:

注意到用的 semantic segmentation 方法是 pixel level 的, 也就是說每個 pixel 都會去判別 yes/no traffic sign. 而我們看到就算是都有 traffic sign 的影像了, 實際上 pixel 是 traffic sign 所占的比例還是偏低, 這讓我開始有點懷疑是否 DNN 有能力分辨出來. 但是….還真的可以!

現在有種感覺, 有時候針對資料不平均做了一些方式讓每個 class 平均一些, 但是 DNN 的效果其實都沒啥提升, 感覺 DNN 對資料不平均的問題較不敏感

不過由於模擬器的 traffic sign 跟 Bosch 的差太多, 因此效果不大好. 我只好加入了一些模器器下的影像去訓練, 結果就好很多了.

但還是遇到一個問題, 我的 macbook 沒有 GPU, 跑一張影像花了 120 secs, 而一秒鐘 camera 會傳來 8 張影像! 根本處理不了, 關鍵是也不知道 Udacity 它們用自己 GPU 跑起來會多快. 所以我就將影像長寬各縮小一半, 總體速度會降到原來的 1/4. 就算如此還是無法驗證是否夠快.

我們團隊卡在這個無法驗證的狀況很久, 導致可能需要用到延長四周的情形. 最後在 teammate Theodore King 的幫助下, 我們使用了 tf 的 object detection API, 使用 mobilenet 速度快到靠北飛起來. 連 CPU 處理一張影像都只需要不到1秒的時間! 何況使用 GPU. 最終總算有驚無險過關了.

~~我之前做那麼辛苦幹嘛~~

閒聊

其實 Udacity 規劃相當棒了, 主要幾個部分都有分別的實作過, 最後來個大一統, 真的很有意思. 但我仍要吐槽的是, 搞環境太麻煩了! 模擬器跑在 virtualbox 上, 而我的 virtualbox window 沒法裝好, 只能裝在 macbook, 但 macbook 又沒有 GPU, 導致使用 deep learning 的方法完全不知夠不夠快! 另外, VM 的環境我還搞不定怎麼跟 host share data, 搞得我只好上傳雲端再下載, 最後衰事接踵而來, VM 也搞不定翻牆 (對, 我在網路長城的牆內)…..80%都在搞環境….真的很痛苦

恩, 終於畢業了… 結束了這漫長的旅程. 原以為我會興奮得不得了, 不過可能是因為最後 project 搞環境太痛苦, 加上這樣子的團隊合作其實沒有約束力 (有兩個完全的壟員), 反而解脫感壓過了高興. 但總結來說, 還是很感謝 Udacity 陪伴了我一年多, 並且有了這麼有趣的經驗! 有機會的話, 我還是會繼續上 Udacity 其他課程的.

ROS 簡介

Self-Driving Car ROS Nodes

Perception Traffic Light

閒聊

Reference