[DAY 26] 利用Python程序码让机器人走出隧道1

前言

大家好，经过昨天对於控制 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 的研究肯定有莫大的帮助。明天开始我们将会开始介绍程序码以及执行的部分，请大家敬请期待。