PX4 vision_to_mavros定位

PX4官方给出以下做法从而使用intel realsense t265深度相机作为视觉估计的硬件选择。

在这里我使用pixhawk 4和realsense t265以及Jetson TX2机载计算机以及benewake tfmini激光测高模块作为室内自主飞行无人机的硬件。

采用Ardupilot官方推荐的vision_to_mavros功能包配置

无人机的双目相机的摄像头朝向正前方，可以将slam融合的高度位姿信息作为EKF2的高度估计信息，具体修改EKF2_HGT_MODE参数为Vision。

我参考以下链接从而配置飞控的相关参数：

Visual Intertial Odometry (VIO) | PX4 User Guide

rviz查看轨迹相关参数：

thien94/vision_to_mavros

rviz可视化坐标具体方法如下：

1. On host computer, open up rviz: rosrun rviz rviz.
2. Add [Path](http://docs.ros.org/api/nav_msgs/html/msg/Path.html), topic name: /body_frame/path to rviz.
3. Change Fixed Frame to target_frame_id, in the case of Realsense T265: camera_odom_frame.

关于EKF姿态解算的部分可以参考PX4官方的PX4-ECL仓库：

PX4/PX4-ECL

该仓库的说明文件如下：

Using the ECL EKF | PX4 User Guide

关于T265坐标转换相关问题：

*把T265定点过程中的坐标变换彻底弄清楚（包含vision_to_mavros代码分析）px4_command T265定点过程的坐标系转换也理清_诗筱涵的博客-CSDN博客

注意：

1. 需要安装realsense的驱动（realsense-ros暂时不需要），这里推荐采用二进制包的方式安装，运行t265_all_nodes.launch脚本之前需要先执行驱动中的一个脚本来创建udev设备驱动规则，也可参考上文中的链接配置。总之，不配置设备驱动规则的话很可能会导致调用设备的时候因为权限等问题报错。

2. vision_to_mavros一定注意realsense设备的安装方向，默认安装方向是竖直向下的，如果需要更改安装方向，一定要在launch文件中修改参数，确保坐标系方向不会错。

3. 运行t265_all_nodes.launch脚本会同时启动mavros脚本，不需要单独启动mavros脚本了。

4. 启动脚本默认采用USB连接飞控串口，如使用TX2的串口连接飞控的时候，一定要手动指定fcu_url为/dev/ttyTHS2:921600。
   举例：

   roslaunch vision_to_mavros t265_all_nodes.launch fcu_url:="/dev/ttyTHS2:921600"
   

5. 该脚本的目的在于：替代传统的通过GPS或光流的方式获得融合EKF姿态数据的方法。飞行模式需要为position/offboard/mission模式下才能使用T265输出的位姿信息，设置错误会导致PreArm检查失败无法起飞。

6. 默认的apm.launch需要修改为px4.launch

已知问题：

1. 使用PX4官方说明中的Auterion/VIO代码会导致odometry消息无法发布
   地面站上面看不到对应的mavlink消息

   错误issue如下：

   No Position data from ODOMETRY. · Issue #15627 · PX4/PX4-Autopilot

   Odometry not showing up in PX4 when using Visual Interial Odometry (VIO)

   解决方案：
   降级PX4固件版本到1.10之后重新运行代码即可（不要降级）

   该问题已有更好的解决办法：使用Ardupilot固件提供的vision_to_mavros代码，launch启动文件中的px4.launch修改为apm.launch

   下载链接如下：

   thien94/vision_to_mavros

2. t265模块速度信息错误

   错误发生于无人机大角度机动飞行时，或环境光线不足导致特征点跟踪丢失时，终端会报错，信息如下

   WARNING [......] (tm-device.cpp:1289) SLAM_ERROR Speed
   

   问题原因：

   PX4 v1.10.1 EKF2 GPS and VIO issues · Issue #15014 · PX4/PX4-Autopilot

   SLAM Error Realsense T265 · Issue #1090 · IntelRealSense/realsense-ros

   T265 linear velocity to /mavros/vision_speed/speed_twist_cov · Issue #18 · thien94/vision_to_mavros

   解决方案：
   （临时办法）AID_MASK采用位置信息融合而不是速度信息融合，设置EKF2_AID_MASK为24
   添加ros节点融合飞控Imu信息：这里采用EKF滤波与数据融合的方法（有待进一步解决）

3. T265 VIO信息与PX4Flow光流信息融合

   Ekf2 T265 and Px4flow fusion

   修改EKF2_AID_MASK参数即可

4. 阿木实验室的VIO版本无人机使用了该vision_to_mavros模块，但是有几个代码参数发生了变化