基于激光雷达的无人机相互避障

本框架是基于激光雷达的无人机群自主避障代码：

• 其主体框架利用ORCA算法，他是经典的多智能体相互避障算法，此版本只能规避动态障碍物，不能规避环境形成的静态障碍物
• 我们对ORVA算法稍作修改，使其可以分布式部署，并且将仿真代码修改为uav实测代码
• ORCA算法需要知道其他智能体的绝对位置和速度作为算法输入，因此我们需要模块【1】和【2】来补充完成
• 模块【2】是利用将lidar安装在当前uav上，并用其检测和估计其他无人机的相对位置和速度
• 因为不满足ORVA算法需要绝对位置和速度的条件，因此需要利用模块【1】来估计当前uav的位置进行结合

1. 基于VIO/LIO的uav自身位置估计

因为无人机安装了Lidar，所以用LIO比较好
还没安装
VIO使用Vins，是基于D435深度相机的，具体安装参考D435i+vins-Fusion+ego-planner+yolo无人机避障实测中vins部分

2. 利用Lidar检测并估计其他uavs位置和速度

代码下载链接：https://gitee.com/dueen1123/lidar_esti

新建一个catkin_ws_lidar的工作空间
在其中先配置好激光雷达livox的SDK以及ROS版本的SDK，参考链接在此
然后将lidar_esti放在同一工作空间一起编译

代码运行如下：

# 启动激光雷达
roslaunch livox_ros_driver2 rviz_MID360.launch
cd ~/catkin_ws_lidar/src/lidar_esti/scripts
# 运行高反无人机检测
python3 multi_highRreflect_detect.py
# lidar到orca转换脚本
python lidar2orca.py

其中multi_highRreflect_detect.py和lidar2orca.py中的DETECT_UAV_NUM变量，需要根据实际uav个数修改

3. ORCA集群避障算法

代码链接：https://gitee.com/dueen1123/orca

实测代码不同的无人机需要添加不同的node_0_true.sh，以及相对应的rvo_node_true.launch和node_0_true.sh

运行如下：

#启动飞机px4飞控
#启动vins绝对定位
#启动lidar相对定位
cd ~catkin_ws_orca/src/rvo_ros/scripts
# 无人机通信
python multirotor_communication.py iris 0
# 无人机悬停
python hover.py iris 1 vel 
# 启动壁障算法
roslaunch rvo_ros rvo_node_true.launch
# 设置目标点
bash node_0_true.sh

============================ 以下可以忽视 ==========================
ORCA是主要的避障算法
ORCA需要一个自定义话题的输入，也就是一个包含速度和位置的消息
但c++和python自定义消息稍有不同，不能直接引用头文件，然后直接用消息名称使用，如下

#include "UAVPosVel.h"
void rvo_true_velCallback(const UAVPosVel::ConstPtr &sub_msg);

首先，#include可能难以直接定位到UAVPosVel.h，需要在cmakelist中的include_directories添加其生成路径，即xxx/devel/include/package_name
其次，UAVPosVel.h中的类不叫UAVPosVel，他生成了一个模板函数，所以使用时应该重新定义
总结如下：

include_directories(include${catkin_INCLUDE_DIRS}${PROJECT_SOURCE_DIR}/include/3d_library  # 确保这里包含了3d_library目录/home/tju/catkin_ws_orca/devel/include/rvo_ros
)

#include "UAVPosVel.h"
typedef rvo_ros::UAVPosVel_<std::allocator<void>> UAVPosVel;
void rvo_true_velCallback(const UAVPosVel::ConstPtr &sub_msg);