Rviz仿真时遇到机器人瞬间回到世界坐标原点的问题及可能原因

1. Problem

仿真过程中，原本正常控制机器人移动，但是突然机器人瞬间回到世界坐标的原点，突破所有限制。
这时应该是算法中某些变量算得了nan，然后计算速度的方程用到了这个带nan的数值，也得到nan的值，通过/cmd_vel这个topic发给机器人端，导致机器人瞬间就回到原点

2. Solution

这时只能在代码中反向排查了，从cmd_vel的数据一步步往上排查，设个判断nan的语句，慢慢检查一下是哪一个变量出问题了，如下：

if(isnan(inter_the[2]))
cout <<"nan appear!"<<endl;

造成这些变量变成nan的原因太多了，很多都是基本计算逻辑问题，只要定位出来就好办了，有时比如给函数input的变量有个0，而这个0用作分母，这就出问题了。

这里只是对Rviz做仿真时出现的一种异常现象的一种可能分析，我遇到这种问题几次，都是由于nan作为速度发出去导致的，所以只要找到算出nan的地方就能快速fix这个bug。