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

前言

大家好，昨天我们稍微讲解了一下在笔者团队的研究中所需的前置步骤，那麽今天我们就来进入程序码的部分吧！

使自走车走出隧道-续

程序码

#!/usr/bin/env python

import math
from robot_control_class import RobotControl

robotcontrol = RobotControl()

t = 1
stop = False
while t >= 0:
  
  scan = robotcontrol.get_laser_full()
  left = min(scan[20:159])
  right = min(scan[200:339])
  d = right - left
  print("d = ", d)

  left_pos = scan.index(left)
  right_pos = scan.index(right)

  pos = 180 - (left_pos + right_pos) / 2
  modify_pos = math.radians(pos)

  left_slant_pos = left_pos - 20
  right_slant_pos = right_pos + 20

  left_slant = scan[left_slant_pos]
  right_slant = scan[right_slant_pos]

  left_side = math.sqrt( left**2 + left_slant**2 - 2 * left * left_slant * math.cos(math.pi / 9))
  left_angle = math.asin( left_slant / left_side * math.sin(math.pi / 9))
  left_angle = math.pi / 2 - left_angle

  right_side = math.sqrt( right**2 + right_slant**2 - 2 * right * right_slant * math.cos(math.pi / 9))
  right_angle = math.asin( right_slant / right_side * math.sin(math.pi / 9))
  right_angle = math.pi / 2 - right_angle

  modify_angle = (right_angle + left_angle) / 2
  angle = round(math.degrees(modify_angle))
  

  if stop == False :

    if (d > 0.2 and d <= 0.75) :

      if (modify_angle < 0) :
        print("modify angle=", -angle)
        robotcontrol.turn_and_move("counter_clockwise", 0.5, modify_angle)

      else :
        print("modify angle=", angle)
        robotcontrol.turn_and_move("clockwise", 0.5, modify_angle)

    elif (d < -0.2 and d >= -0.75):

      if (modify_angle > 0) :
        print("modify angle=", angle)
        robotcontrol.turn_and_move("clockwise", 0.5, modify_angle)

      else :
        print("modify angle=", -angle)
        robotcontrol.turn_and_move("counter_clockwise", 0.5, modify_angle)

    elif (d > 0.75) :
      modify_angle += math.pi / 18
      print("modify angle=" , angle)
      robotcontrol.turn_and_move("clockwise", 0.5, modify_angle)

    elif (d < -0.75) :
      modify_angle += math.pi / 18
      print("modify angle=", -angle)
      robotcontrol.turn_and_move("counter_clockwise", 0.5, modify_angle)

    else :
      print("modify pos=", pos)
      robotcontrol.turn_and_move("clockwise", 0.5, modify_pos)

  else :

    if t <= 8 :
      x = 0.5 - t * 0.0625
      print("slow down , modify pos=", pos)
      robotcontrol.turn_and_move("clockwise", x, modify_pos)

    else :
      print("stop")
      robotcontrol.turn_and_move("clockwise", 0, 0)

  t = t + 1

  if t > 10:
    t  = 1
    stop = not stop

程序码解析

首先，我们先看到以下这段

  scan = robotcontrol.get_laser_full()
  left = min(scan[20:159])
  right = min(scan[200:339])
  d = right - left
  print("d = ", d)

  left_pos = scan.index(left)
  right_pos = scan.index(right)
  
  pos = 180 - (left_pos + right_pos) / 2
  modify_pos = math.radians(pos)

这边就是刚刚提到的找出左右两侧的最短距离(left, right)，以及最短距离对应的角度(left_pos, right_pos)，范围取20~159以及200~339只是为了约略选出自走车左侧与右侧的雷射扫描范围。变数 pos 算出来的就是目前的前行方向相对於隧道中线的角度。(以下图为例，假设左侧角度为80度、右侧为260度，pos 之值为10，表示目前的前行方向在中线左侧10度)，由於 waffle 的角速度单位为 rad/s，因此下一行再将 pos 转换为弧度。变数 d 用来判断目前自走车的位置是偏中线左侧还是右侧。

  left_slant_pos = left_pos - 20
  right_slant_pos = right_pos + 20

  left_slant = scan[left_slant_pos]
  right_slant = scan[right_slant_pos]
  
  ...
  
  modify_angle = (right_angle + left_angle) / 2
  angle = round(math.degrees(modify_angle))

这一大串的意义在於因为几次模拟下来，透过 modify_pos 所修正的幅度较小，当自走车行进到弯道处时偶尔就会来不及跟上弯道的曲率而撞墙，因此笔者团队决定再利用两斜边来找出一修正角。虽然过程有点冗长但其实只是大量的三角函数关系，为节省篇幅这边就先不解释。但值得注意的是，modify_pos 之值有正负之分，但 modify_angle 的值皆为正值，是因为过程中使用 asin 的关系，因此在使用时需要注意。

在讲解一大串的 if 判断式之前，我们先来看看外面的部分：

t = 1
stop = False
while t >= 0:

...

  if stop == False :
  
  ...
  
  else :

    if t <= 8 :
      x = 0.5 - t * 0.0625
      print("slow down , modify pos=", pos)
      robotcontrol.turn_and_move("clockwise", x, modify_pos)

    else :
      print("stop")
      robotcontrol.turn_and_move("clockwise", 0, 0)

  t = t + 1

  if t > 10:
    t  = 1
    stop = not stop

由於笔者团队之研究主题为自走车搭配点云雷达扫描隧道，因此希望自走车每走一段路程後便能够停下来，所以便利用了 bool 来判断当前是否需要停下。可以看到初始之 stop 为 False，每当执行一次回圈，变数 t 便会+1，直到大於10时便会变换 stop 当中的 bool 值。当 stop 为 True 时，便会将前行速度的参数值 x 逐渐减少至0，达到让自走车停下的效果。

至於为什麽不直接在第一个 else 当中使用函式 stop_robot(self) 呢?这是因为笔者团队发现每次执行函式 stop_robot(self) 後，自走车在接近停下时就会神秘的抖动一下，导致自走车的前行方向偏离隧道中线非常多，甚至有时候会直接转头过去面向隧道墙壁。笔者团队试过调整 class 当中的各项参数值、修改函式等等，但还是没办法解决这个现象，实在是让笔者团队相当头痛，因此想出这个让自走车逐渐停下的方法，实际测试後也确实与原本执行 stop_robot(self) 还要好很多。当然或许还有某些地方是笔者团队所疏漏的，因此目前在这部分就只能先以此方法来作为解决方案，待日後详细厘清原因後或许就能改善此现象。

接下来就进入 if 判断式当中的内容了，但如果要以文字慢慢解释每一行也不容易，因此这边笔者就以下图来解释：

在 if 判断式当中可以看到每个条件式都与 d 有关，这是因为要让自走车判断当前与隧道的相对位置，若是为图中左侧的情形，就视为当前隧道为笔直段，使用 modify_pos 修正；至於中间所代表的则为已偏离中线许多，将隧道视为弯道段，使用 modify_angle 进行修正；右侧则代表已偏离隧道中线非常多，因此将 modify_angle +10度後进行修正，达到快速回到中线之效果。後方将 modify_angle 转成角度仅是作为输出到 Terminal 上时较好读懂度数而已。

至於在部分 elif 下还有再以 modify_angle 做为判断的 if 函式，可由下图来解释：

其实最主要就是在判断当前自走车是驶离还是修正，我们可以简单用上图来描述不细分方向的结果：若自走车呈左侧之情形，也就是正驶离中心线的情形，由於 d 判断出目前在中线左侧，若函式中统一使用 "clockwise" 做修正的话，那麽确实会将前行方向往中线修正。但如果今天已经进入了右侧之情形，照理说已经符合预期的修正没错，但由於上方所提到的 modify_angle 皆为正值，因此自走车还会继续顺时针前进，那麽就会使前行方向再度偏离隧道中线，接着进入中线右侧之修正式，再过度修正，如此反覆。这样会让自走车本身的行进路线呈S型，使整体路径变长且更耗费能源，因此针对前行方向再次做细分的优化是有必要的。

程序码大致就介绍到这边，实际执行程序後，自走车就会慢慢的走出隧道啦!但由於隧道本身太长加上 waffle 的行走速度不快，因此笔者在最後就只放上 waffle 隧道中行走中的图片，可以看到 Terminal 上持续列印出变数 d 、修正角度 modify_pos 或者 modify_angle 以及停下时的各种字串。

结语

这两天介绍了控制隧道内自走车的研究，整体来说比较不像是教学而是解说笔者团队的研究成果，虽然可能还有很多地方做得不够好需要再更加优化，但愿能与更多更了解更 ROS 架构的前辈们有交流的机会。对於不熟悉 ROS 的读者来说，相信也能从这两天的介绍当中多少学到一些 ROS 的相关知识。明天要介绍的内容同样也是与 Gazebo 有关，讲述的是如何在 Gazebo 当中获取机器人的相对座标资讯，请各位敬请期待!