XTDrone vins-fusion+ego-swarm debug（已解决）

问题描述：

低优先级无人机（除0号无人机以外）运行撞墙；启动时低优先级无人机会异常的飘逸一段距离，而rviz里显示的b样条曲线的控制点起点没变。
这是刚启动的情况

启动大概不到1秒无人机开始漂移

最后扒窝

问题分析：

1. 由于0号无人机在ego-swarm里的运行特点是优先级最高，不用考虑其他无人机的飞行轨迹，既然0号无人机运行正常，出现问题的原因可能是集群运动规划部分
2. 可以比对以下ego相关的输入输出数据，看一下是不是由于输入输出导致的这个问题
3. 由于XTDrone的飞控是px4和mavros，还有可能是mavros传输数据的异常

问题探究：

ego的输入相关部分，主要是vins的输出

在XTDrone集群无人机路径规划的逻辑中，vins输出的两个量（vins_estimator/camera_pose和vins_estimator/odometry）都要被vins_transfer进行转换，主要是为vins的初始位置加一个偏移量，之后ego_transfer会对vins_transfer转换后的camera_pose进一步处理得到：/iris_$(drone_id)/odometry和/iris_$(drone_id)/camera_pose。对ego关于vins的传入数据部分的分析主要是这两个量，就extrinsic好像是无人机相机和机体之间的转换，是一个不变的量。这里还需要用到gazebo里无人机模型的真实位置，XTDrone给的获取真实位置脚本好像有点问题这里我改了一下：
下边就是录个bag包用python脚本解包分析了
我是用的苯法子一帧一帧在debug模式下看的

可以看到这两个topic的位置是一致的都是1号无人机的初始位置，并且和ground_true给的位置真值一致，一直到bspline曲线出现的时刻（路径规划开始的时候）都是没有问题的，orientation差了一个四元数，这个数好像是记在extrinsic里边了。
并且在位置真值开始出现漂移的时候vins给的这两个位置不变，因此出现问题不是vins的输出导致的。
由此bag包顺势向下看数据流，以位置真值开始漂移时刻为基准观察bspline曲线里的控制点以及由bspline曲线控制点经由ego给的控制服务器发布的/xtdrone/iris_1/cmd_pose_enu的值

在异常位移开始时刻这些控制量都是正常的

ego的集群路径规划部分

ego的集群路径规划是通过两种传输高优先级路径所实现的：

1. 链式网络的形式：无人机会接受比他高一级优先级的无人机所发布的multi_bspline（就是bspline的可变长数组，在消息头文件里边是用vector实现的）数据，将自己规划的bspline插入进去然后再发布。
2. 广播网络形式：将自己规划的路径通过/broadcast_bspline发布出去
   

对这部分的调试也很简单，把上边代码注释掉就好了，先是把链式网络部分注释掉，没用没报错，之后又将广播网络注释调，没用有报错。

mavros部分

对mavros部分的调试我的思路是将gazebo中无人机数量将到一个，将无人机编号由0号改为1号，再运行vins-fusion以及ego-swarm观察效果，如果正常，说明mavros部分没有问题。最开始我遇到了无人机通信不正常的错误，就是说mavros连接不正常。

最后发现是我mavlink_udp_port和mavlink_tcp_port这两个端口号忘记改了，改过之后就正常了，之后的运行都是没有问题的mavros部分正常。

问题产生原因与解决方法

ego给的控制位点太正常了，规划的路径是在world坐标系的逻辑下规划出的控制位点，而communication脚本接受控制位点控制无人机是在以无人机起飞时刻为原点的坐标系下，差了一个偏移量，把这段偏移量加上就好了。

这是我直接加到communication脚本里的测试代码，加上之后1号无人机就没有问题了。
当然这不是最好的解决方法，最好的解决方法应该是自己写一个转换脚本，或者直接给ego启动的时候传一个偏移量，让ego自己转换