前言

大家好，經過昨天對於控制 waffle 機器人程式碼的介紹，相信大家都已經瞭解了相關的基本函式。不過筆者想要在這邊先補充一個細節，那就是昨天在撰寫 test.py 來嘗試操控我們的 waffle 時，為什麼我們沒有像之前的教學一樣建立 Node、Publisher 或 Subscriber 呢?原因其實就在於我們在 test.py 中 import RobotControl 這個 class，而這個 class 當中就已經建立 robot_control_node 這個節點了，所以我們也就不用在檔案中重新定義。假如今天我們需要分批寫出很多不同的執行檔，只要好好利用 class 就可以省下很多時間，也能讓執行檔本身越加簡潔。

回到正題，今天要介紹的是筆者的研究成果，也就是透過 ROS 系統控制自走車，使自走車走行走於隧道中線並且順利走出隧道。由於筆者對於 ROS 模型建構的基礎較薄弱，因此我們就直接透過 turtlebot3 的 waffle 來進行模擬，這也是為什麼先前都以 waffle 做介紹的原因。由於整體篇幅有點長，因此會分成兩天來進行介紹，那麼以下就開始介紹今天的主題。

使自走車走出隧道

修正函式庫

在介紹程式碼之前，為了使程式更符合我們 ROS 專案的需求，因此筆者有在 robot_control_class.py 當中的某些部分做修改，在此先行告知大家：

• 為使雷射回傳的距離更加即時，因此我們將 self.rate = rospy.Rate(10) 當中的數值調做調整，希望減少得到距離資訊與執行修正式之間的誤差，修改後如下：

self.rate = rospy.Rate(200)

• 於 stop_robot(self) 當中添加自變數 self.rate = rospy.Rate(1)，並於尾行添加 self.rate.sleep()，修改後如下：

    def stop_robot(self):
        self.rate = rospy.Rate(1)
        #rospy.loginfo("shutdown time! Stop the robot")
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        self.publish_once_in_cmd_vel()
        self.rate.sleep()

會做此修正是因為希望在執行函式 stop_robot 後讓自走車確實停下，避免停下的時間過短而使接下來的前行函式覆蓋掉 stop_robot 的指令。

• 由於隧道本身可能會有一定的曲率，因此自走車需要能一邊前行並同時修正前行角度的功能，但原 class 當中沒有能夠符合需求的函式，所以需要新增函式如下：

    def turn_and_move(self, clockwise, x, z):

        # Initilize velocities
        self.cmd.linear.x = x
        self.cmd.linear.y = 0
        self.cmd.linear.z = 0
        self.cmd.angular.x = 0
        self.cmd.angular.y = 0

        if clockwise == "clockwise":
            self.cmd.angular.z = -z
        else:
            self.cmd.angular.z = z
        
        self.rate.sleep()
        self.vel_publisher.publish(self.cmd)

簡單來說就是給自走車一個前行速度與角速度，相信函式本身並不難理解。

修正機器人前行角度的方法

上面有提到，為了應付隧道本身的曲率，所以我們必須一邊前行並修正前行角度。但是自走車本身只會帶有前行速度與角速度的資訊，對於前行方向的正確與否是人為主觀判斷的，為此我們必須想辦法讓自走車知道自己前進的方向是否正確。經過筆者團隊的構思，使用的是透過距離來算出修正角的方法，以下利用圖片簡略說明之：

首先，先使自走車透過雷射掃描獲取所有方位的距離資訊，接著利用這些數據找出左右最短距離所對應之角度，並且推算出當前自走車前行的方位。當前圖片中顯示的是隧道為筆直、自走車前行方向朝隧道正前方，且自走車位於隧道中線的情形，但若是自走車開始進入彎道，造成自走車前行方向開始偏離隧道正前方呢?我們再用一張圖來解釋：

從這張圖可以看到，當進入彎道時自走車根據最短距離所對應之角度得出修正角，此時再透過函式 turn_and_move 來進行角度的修正。

結語

今天我們先針對了函式庫本身進行修改與擴充，然後再介紹了筆者團隊對於修正機器人前行角度的方法。其實最主要的重點在於前者，只要瞭解相關參數如 rospy.Rate(10)、cmd.linear.x、cmd.linear.z 的應用，那麼就能寫出更符合自己 ROS 專案需求的函式庫，更甚至能夠自己編譯一個 class 供整個專案來使用，對於日後 ROS 的研究肯定有莫大的幫助。明天開始我們將會開始介紹程式碼以及執行的部分，請大家敬請期待。