如何把t265定位信息传入pix4 1.13版本及以下飞控

本文介绍如何把t265的视觉odom信息，传入到px4飞控作为定位信息源，当然该方法也的odom信息源不一定是t265，可以是其他定位算法或者传感器。

1.T265基本信息

T265使用了双目鱼眼相机 分辨率848X800分辨率 30HZ 单色图像 视场角 163° Fov(±5°)，IMU型号为 BMI-055。
T265输出的里程计（话题：/camera/odom/sample）的坐标系与官方提供的图片坐标系有不同。实际测量到的坐标系为，正前方为X_odom轴正方向、左边为Y_odom轴正方向、天向为Z_odom轴正方向。恰好与飞控中的FLU坐标系重合，所以不用做任何旋转处理，可以直接传入飞控。

2.驱动安装

2.1 librealsense SDK安装

下载源码(注意版本一般是v2.50.0)

git clone -b v2.50.0 https://github.com/IntelRealSense/librealsense.git

进入源码目录

cd librealsense

安装依赖

sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade
sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev at

从libalsense根目录运行Intel Realsense权限脚本

./scripts/setup_udev_rules.sh

注意拔出t265，然后按下回车。(Remove all RealSense cameras attached. Hit any key when ready)
编译

mkdir build && cd build
cmake ../ -DBUILD_EXAMPLES=true -DFORCE_RSUSB_BACKEND=true -DCMAKE_BUILD_TYPE=release -DBUILD_GRAPHICAL_EXAMPLES=FALSE

安装

sudo make uninstall && make clean && make -j8 && sudo make install

2.2 ROS包安装

下载源码并编译

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/
git clone -b 2.3.2 https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
cd ..6

编译

cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install

2.3 t265 的使用

运行下面的指令直接开启t265:

roslaunch realsense2_camera rs_t265.launch

但是我在运行的时候，果然报错了，根本没有找到设备，运行

rs-enumerate-devices

也没有找到设备—绝望
找了一大堆资料才发现，编译librealsense时TM2没有编译成功，因为我这块板子的网络不是很好，所以直接给我跳过了，输出的编译信息没有爆红，而且被顶上去了，就没有注意到，所以在编译时一定要注意TM2是否成功编译，不然t265会出问题。
再次运行之后，就可以得到t265的定位话题（/camera/odom/sample）。

3.传入飞控

将定位数据传入飞控有两种方法，一种是通过ros话题传入飞控，一种是通过tf关系传入飞控。这两种方式不仅可以传入t265的定位数据，还可以传入各种slam定位数据，方法也是一样，只是话题或者tf名称不一样而已。

3.1 ros话题方式

我使用的时pix1.13.3的固件，1.14以上这些参数应该就变了，请仔细分辨。
首先要在QGC地面站中把EKF2的参数设置一下，直接搜索参数ekf2_aid_mask，把它改为使用vision的位置和yaw值，即改为24，一般不用视觉速度信息，用飞控的里面的传感器就可以了。

可以这里设置t265与飞控的偏置值（EKF2_EV_POS_X、EKF2_EV_POS_Y、EKF2_EV_POS_Z），这样就可以修正t265与飞控之间的偏差，标定外参可以看我的另一篇教程：

这样飞控会默认使用/mavros/odometry/out话题的定位数据（好像/mavros/vision/pose也可以），所以下一步就是写一个node把t265的数据传入到该话题。当然，这个节点也不用我们写，有现成的:GitHub - pedrogasg/VIO_bridge: Interface PX4 with Realsense T265，但是这种方式相比于tf方式，有点麻烦。下面是更加简单的方式。

3.2 TF方式

3.2.1 QGC设置

我使用的时pix1.13.3的固件，1.14以上这些参数应该就变了，请仔细分辨。
首先要在QGC地面站中把EKF2的参数设置一下，直接搜索参数ekf2_aid_mask，把它改为使用vision的位置和yaw值，即改为24。

可以这里设置t265与飞控的偏置值（EKF2_EV_POS_X、EKF2_EV_POS_Y、EKF2_EV_POS_Z），这样就可以修正t265与飞控之间的偏差:

这样飞控会默认使用/mavros/odometry/out话题的定位数据（好像/mavros/vision/pose也可以），这里不使用默认的话题方式，改成聆听tf关系的方式：

3.2.2 参数文件设置

在mavros的功能包中，找到px4_config.yaml

roscd mavros && cd launch 
sudo gedit px4_config.yaml

这里面有很多参数，可以看看各部分的标签，就可以看懂了，这里改动vision_pose_estimate，把false改成true。然后这里frame_id和child_frame_id需要改成t265的tf关系，

t265启动会默认发出tf，如果没有发出，可以在启动t265的rs_t265.launch文件中开启，最后一个publish_odom_tf参数。

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default="t265"/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>

  <arg name="fisheye_width"       default="-1"/> 
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye1"     default="false"/>
  <arg name="enable_fisheye2"     default="false"/>

  <arg name="fisheye_fps"         default="-1"/>

  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
  <arg name="enable_pose"         default="true"/>

  <arg name="enable_sync"           default="false"/>

  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="initial_reset"         default="false"/>
  <arg name="reconnect_timeout"     default="6.0"/>
  <arg name="unite_imu_method"      default=""/>

  <arg name="publish_odom_tf"     default="true"/>

然后可以找找t265的tf名字

rostopic echo /tf

查看有没有关于t265的tf，把相应的名称填到px4_config.yaml中。然后重启px4飞控，可以通过echo话题验证是否成功。

rostopic echo /mavros/local_position/pose

4.报错解决 :

4.1 Could not find a package “ddynamic_reconfigure”

CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by
  "ddynamic_reconfigure" with any of the following names:
   ddynamic_reconfigureConfig.cmake
   ddynamic_reconfigure-config.cmake

解决:

sudo apt install ros-noetic-ddynamic-reconfigure