Task

本次实验才采用“Follow the carrot 算法去跟随定义好的路径。控制的目标点在机器人行走的路径上，距离机器人是个常数距离。机器人计算自己的方向角和目标点角度之间的相差度数。控制这个差角为0.
为了在探测时候能够用这个算法，路径需要在实时更新并且需要配合避障，因为在探测时候路径计算的并不精确，机器人有碰撞到障碍物的可能性。

算法

位置和向量

向量的模：

<script id="MathJax-Element-1" type="math/tex; mode=display">||\boldsymbol{v}||=\sqrt{x^{2}+y^{2}}\tag{vector-norm}</script>
点乘，加法，减法：
<script id="MathJax-Element-2" type="math/tex; mode=display">\boldsymbol{v}\cdot a = (x_{v}a;y_{v}a)\tag{mult-vector}</script>
<script id="MathJax-Element-3" type="math/tex; mode=display">\boldsymbol{v}+\boldsymbol{u} = (x_{v}+x_{u};y_{v}+y_{u})\tag{add-vector}</script>
<script id="MathJax-Element-4" type="math/tex; mode=display">\boldsymbol{v}-\boldsymbol{u} = (x_{v}-x_{u};y_{v}-y_{u})\tag{add-vector}</script>

处理输入输出

控制机器人采用Twis消息类型
从激光输入进来的数据可以用来避障
输入Path的消息格式：

std_msgs/Header header
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
geometry_msgs/Pose pose
geometry_msgs/Point position
geometry_msgs/Quaternion orientation

本次实验中只有x、y坐标下的位置，路径存储在一个队列对象MyPoint中，你可以仅仅为每一个在poses向量中的元素建立一个新的MyPoint类的对象，然后添加到路径队列中。每次添加新的控制点需要删掉之前的点。在这种情况下，每一次循环路径都会改变，每次都是跟随最新的路径。将每一个路径变换带Odometry坐标下。

路径

移除掉前面的点存储在lastRemoved中。之后需要移除掉路径上被认为已经到达的点，这一步要防止机器人沿着路径后退。
把移除掉的点存储在lastRemoved中。如果没有点移除掉机器人首先去lastRemoved的移除点，否则机器人在lastRemoved和 路径前端之间的线段上寻找目标点。

控制点（control point）

控制点是路径上线和弧的插值点。为了避免计算和选择插值点，估计目标点方法如下：

<script id="MathJax-Element-5" type="math/tex; mode=display"> d=||\vec{l}-\vec{a}+\vec{s}\cdot2^{-i}+\vec{v}||\tag{approx-condition} </script>

l⃗  <script id="MathJax-Element-6" type="math/tex">\vec{l}</script>:上一次从路径上移除掉的点
s⃗  <script id="MathJax-Element-7" type="math/tex">\vec{s}</script> = f⃗ −l⃗  <script id="MathJax-Element-8" type="math/tex">\vec{f}-\vec{l}</script> f⃗  <script id="MathJax-Element-9" type="math/tex">\vec{f}</script>:是路径队列的首部值
v⃗  <script id="MathJax-Element-10" type="math/tex">\vec{v}</script> : 前一次迭代的贡献值？？
a⃗  <script id="MathJax-Element-11" type="math/tex">\vec{a}</script>：机器人坐标
d <script id="MathJax-Element-12" type="math/tex">d</script>： 目标和机器人之间的距离
i<script id="MathJax-Element-13" type="math/tex">i</script> : 迭代次数

如果d小于targetDistance， s⃗  <script id="MathJax-Element-14" type="math/tex">\vec{s}</script>， v⃗  <script id="MathJax-Element-15" type="math/tex">\vec{v}</script>相加

<script id="MathJax-Element-16" type="math/tex; mode=display">\vec{v}_{i+1}=\vec{v}_{i}+\vec{s}_{i}\cdot2^{-i}\tag{approx-passed}</script>

大概迭代10次你会有一个精确的（ ∼2−10⋅||s⃗ || <script id="MathJax-Element-17" type="math/tex">\sim2^{-10}\cdot||\vec{s}||</script>）目标点的估计值:
t⃗ =l⃗ +v⃗ (target-point) <script id="MathJax-Element-18" type="math/tex">\vec{t}=\vec{l}+\vec{v}\tag{target-point}</script>
如果机器人距离路径很远，可能要移除的不仅仅是第一个距离大于targetDistance的点， v⃗  <script id="MathJax-Element-19" type="math/tex">\vec{v}</script>保持为0，机器人朝向第一个被移除的点的移动。

如果路径点之间相距比较远，机器人所处的位置如图所示，机器人会找到两个插值点，有可能会导致机器人后退。简单的解决方法是把目标点设置为：移除点和路径队列首项之间的线段 与 过机器人原点的法线 的交点
xv=||a⃗ −l⃗ ||⋅cos(φf⃗ −l⃗ −φa⃗ −l⃗ )||s⃗ ||⋅xs(intersection-x) <script id="MathJax-Element-20" type="math/tex">x_{v} = \frac{||\vec{a}-\vec{l}||\cdot\cos(\varphi_{\vec{f}-\vec{l}}-\varphi_{\vec{a}-\vec{l}})}{||\vec{s}||}\cdot x_{s}\tag{intersection-x}</script>
yv=||a⃗ −l⃗ ||⋅cos(φf⃗ −l⃗ −φa⃗ −l⃗ )||s⃗ ||⋅ys(intersection-y) <script id="MathJax-Element-21" type="math/tex">y_{v} = \frac{||\vec{a}-\vec{l}||\cdot\cos(\varphi_{\vec{f}-\vec{l}}-\varphi_{\vec{a}-\vec{l}})}{||\vec{s}||}\cdot y_{s}\tag{intersection-y}</script>
φf⃗ −l⃗  <script id="MathJax-Element-22" type="math/tex">\varphi_{\vec{f}-\vec{l}}</script>是 f⃗ ,l⃗  <script id="MathJax-Element-23" type="math/tex">\vec{f}, \vec{l}</script>之间的角度， φa⃗ −l⃗  <script id="MathJax-Element-24" type="math/tex">\varphi_{\vec{a}-\vec{l}}</script>是 a⃗  <script id="MathJax-Element-25" type="math/tex">\vec{a}</script>, l⃗  <script id="MathJax-Element-26" type="math/tex">\vec{l}</script>之间的角度

角速度

φ=φt−a−α,(target-angle) <script id="MathJax-Element-27" type="math/tex">\varphi=\varphi_{\boldsymbol{t}-\boldsymbol{a}}-\alpha,\tag{target-angle}</script>

<script id="MathJax-Element-28" type="math/tex; mode=display">\omega=k\cdot\varphi</script>
k是常值1/t（t 是返回0值的时间），k越大对路径变化的反应越快速，但是太大会导致不稳定，建议设置为2。

避障

绿色区域：设置机器人速度为原先的80%，误差角度设置为：

<script id="MathJax-Element-29" type="math/tex; mode=display">\varphi=-0.3\cdot sign(\beta)\tag{avoid-front}</script>
蓝色区域：设置机器人速度为原先的60%，误差角度设置为：
<script id="MathJax-Element-30" type="math/tex; mode=display">\varphi=-0.2\cdot sign(\beta)\tag{avoid-side}</script>
红色区域：设置机器人速度为原先的60%，误差角度设置为:
<script id="MathJax-Element-31" type="math/tex; mode=display">\varphi=-0.3\cdot sign(\beta)\tag{avoid-front}</script>

code

https://github.com/will1991/rosintrodution

CMakeLists.txt