PX4避障和轨迹规划（3DVFH*）

采用伴侣计算机(Companion Computer)的方案，通过使用mavros的obstacle_distance插件订阅/mavros/obstacle/send话题，进而通过local_planner本地轨迹规划器实现局部路径规划。

参考如下链接：

官方说明：

Collision Prevention | PX4 User Guide

知乎上面搜的：

【px4】PX4/avoidance

PX4官方避障策略代码（基于ROS运行）：

PX4/PX4-Avoidance

这里需要在mavros外部控制策略条件下，添加障碍物感知节点，并将/scan话题重映射到/mavros/obstacle/send这里

以下issue是关于如何添加LaserScan节点到mavros的（暂时不推荐使用，目前问题还未解决）：

mapping LaserScan to obstacle_distance mavlink message · Issue #1082 · mavlink/mavros

注意：查看数据是否成功显示，可以通过查看mavlink数据流中的obstacle_distance对应的内容。

关于地面站是否能获得到障碍物距离信息相关问题：

mavros通过伴侣计算机发送的mavlink数据流传输对应的信息与第三方传感器直接通过串口连接到飞控的原理不完全一样，硬件通过串口连接到飞控是直接发送距离对应的信息，由飞控完成从距离信息到mavlink数据流的转换，而通过伴侣计算机发送的mavlink数据流则可以被飞控直接识别，因此地面站可以通过上述两种方式感知到障碍物距离信息。

关于Offboard模式下的避障策略相关问题：

Obstacle Avoidance | PX4 User Guide

硬件上机测试参考如下链接：

已知相关问题：

1. 仿真的launch启动文件对应的px4_config.yaml路径不正确，需要手动改到avoidance对应的目录下。（代码更新后该路径已正确）

2. D435跟官方提供的aeroD415启动文件一样，可以直接用这个D415的启动文件启动。（这个启动脚本不需要手动输入相机的序列号）

3. 官方声明中提到的generate_launchfile.sh.deprecated这个脚本已经被废弃，但是该脚本生成的启动文件仍然能正常运行（不明白官方为什么要废弃该脚本）。

4. avoidance避障节点启动的标志是在地面站上看到avoidance node is up信息（新版地面站点击左上角的绿色或黄色标志即可看到avoidance节点是否正常运行）。

5. 深度相机安装方向为前方，无人机只能往相机视野范围内移动，因此飞机在mission/offboard模式下只能朝前方移动或者是偏航。其他方向无障碍物信息，飞机不可移动，摇杆操作滚转/后退均无效。具体查看障碍物信息可通过查看/mavros/obstacle/send信息。
   参考如下说明：

   it depends on the field of view of your sensors, at which angles do you get sensor coverage? And how is the data reported, e.g do you get one value per sensor at each timestep or do you actually get a range scan per sensor (and what would be the resolution of that).
   
   But as an example:
   I assume you have 6 sensors with fov 10deg, which all report one value per timestep. sensor 1 is located to the front of the drone and they are evenly spaced.
   
   then you should fill the message as follows:
   msg.frame = MAV_FRAME_BODY_FRD
   msg.increment = 10 (degrees, resolution)
   msg. angle_offset = -5 (degrees to shift, such the the fov of the first sensor is centered to the front of the drone)
   msg.distances = [data1, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX
   data2, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
   data3, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
   data4, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
   data5, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
   data6, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX]
   
   put UINT16_MAX where you do not have any sensor coverage. The drone will refuse to fly these directions
   msg.
   

6. 关于TF坐标相关ERROR解决办法
   错误描述如下所示：

   TF Buffer: could not retrieve requested transform from buffer, unregistered
   

   issue链接如下：

   TF Buffer error when launching local_planer · Issue #471 · PX4/PX4-Avoidance

   解决办法如下：默认配置文件对应的fcu_url为127.0.0.1:14550，这里需要修改为/dev/ttyTHS2:921600
   如果该ERROR持续显示，一定要检查机载电脑与飞控连接情况。

7. realsense t265和d435脚本合并问题
   avoidance节点调用的为rs_rgbd.launch，该脚本包含了realsense相机启动文件和相关的点云信息的调用。vision_to_mavros节点调用的是rs_t265.launch脚本，这两个脚本同时运行的时候会报错realsense_camera_manager，导致一个节点意外终止。
   解决办法：采用tools文件夹下的generate_launchfile.sh.deprecated脚本手动生成对应的配置文件，之后在配置文件中手动添加T265位姿对应的相关内容。（该做法虽然官方不推荐，但到目前为止官方并未说明如何修改已有的配置文件）

8. USB3.0与GPS等机载设备干扰问题
   使用USB2.0或2.1的HUB或线缆，防止产生干扰。

9. 飞行器当前视深度相机检测到障碍物时，飞行器只能操作高度、前进，和YAW。

10. 避障系统提供途中关闭节点的功能，具体怎么做有待进一步查询。