3. ROS编程入门—Wall Following

Introduction：

本次设计的任务是让机器人沿着墙按照固定距离走

算法

第一步是找到墙，利用激光雷达发现最近障碍物的角度和距离。
根据墙的位置保持固定的距离和固定的角度，可以用两个PID控制器来实现这两个参量的控制。

输入输出处理

程序的输入是激光雷达的数据，输出的是控制Twist消息
每一个激光点对应的实际角度：

$$\varphi=\left(i-\frac{l}{2}\right)\cdot\theta\tag{angle-ranges}$$

$\varphi$：第$i$个元素对应的角度
$i$：向量元素的索引
$l$：向量长度
$\theta$: 角度增量

如果在整个ranges向量空间中寻找最小值，有可能让机器人在狭窄的空间中跟随对面的墙(译者：不是很懂)。为了确保不会发生这种状况，在合理的一般范围内查找最小值。查找的索引起始和终止值可以通过下面的式子调整：

$$i_0 = l\cdot\frac{d+1}{4}$$

$$i_n = l\cdot\frac{d+3}{4}$$

$d$：方向（1：跟随左边的墙，-1：跟随机器人右边的墙）