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。