athena/Blimp-Autopilot-Ascend/README.md

# 代码

## 结构
在Atlas 200I DK A2开发板上的代码结构如下：
- blimp-autopilot
    - src
        - ego-planner
        - mavros
        - vins-fusion
    - shfiles
    - devel（编译时自动生成）
    - build（编译时自动生成）
    - README.md

其中，blimp-autopilot建议放在home目录下（如使用root登录，则可以放在root下）

由于实际运行的时候飞艇在空中不方便接HDMI线，无法直接运行开发板上的可视化组建，因此可以通过配置多机版ROS，通过局域网连接在在本地计算机上可视化显示里程计、建图、规划效果，具体操作为：

将/src/vins-fusion/utils/l路径下的odom_visualization、pose_utils、quadrotor_msgs这三个目录拷贝至本地进行编译，然后在配置好多机ROS相关IP的情况下，在本地启动`roslaunch odom_visualization vins_vis.launch`

在本地运行可视化的工程目录结构：
- odom_vis_ws
    - src
        - odom_visualization
        - pose_utils
        - quadrotor_msgs

然后在odom_vins_ws路径下执行`catkin_make`即可。编译成功后将`source`此工程`build/setup.bash`的语句加入`.bashrc`即可。

## 依赖

### Mavlink

此工程中包括mavros，但对其中一个文件vision_pose_estimate.cpp做了修改，编译此工程即可提供mavros相关功能。

mavros依赖mavlink，二进制安装mavlink的方式为
```
sudo apt-get install ros-noetic-mavlink
```
安装完成之后使用`rospack list`查看是否已经有mavlink

### Ceres

参见：

https://zhuanlan.zhihu.com/p/460685629

### Boost_python3

参见：

https://blog.csdn.net/zsf10220208/article/details/102571611#:~:text=%281%29%20%E5%8E%BBboost%E5%AE%98%E7%BD%91%20https%3A%2F%2Fwww.boost.org%2F%20%E4%B8%8B%E8%BD%BD%E8%BE%83%E6%96%B0%E7%89%88%E6%9C%AC%E7%9A%84boost%2C%E5%A6%82boost_1_71_0.zip.%20%282%29%20%E8%A7%A3%E5%8E%8B%3A%20unzip%20boost_1_71_0.zip,sudo.%2Fb2%20install%20%28d%29%20sudo%20ldconfig%20%284%29%20%E7%BC%96%E8%AF%91%E6%88%90%E5%8A%9F%E5%90%8E%E5%8F%AF%E4%BB%A5%E5%9C%A8%E9%BB%98%E8%AE%A4%E7%BC%96%E8%AF%91%E8%B7%AF%E5%BE%84%2Fusr%2Flocal%2Flib%E4%B8%8B%E6%89%BE%E5%88%B0%E7%9B%B8%E5%85%B3%E7%9A%84libboost%2A.so%2C%20boost%E5%9C%A8%2Fusr%2Flocal%2Finclude%2Fboost.

https://boostorg.jfrog.io/artifactory/main/release/1.71.0/source/

### OpenCV 3.4

https://github.com/opencv/opencv/tree/3.4

下载源码，解压之后进入目录，新建build目录，在build目录下执行
```
cmake .. -DCMAKE_BUILD_TYPE=RELEASE -DCMAKE_INSTALL_PREFIX=/usr/local/opencv34 -DWITH_LIBV4L=ON -DWITH_CUDA=OFF -DWITH_TBB=ON -DWITH_OPENMP=ON -DWITH_OPENGL=ON
make -j8
sudo make install
```

### RealsenseSDK

https://github.com/IntelRealSense/librealsense/releases

下载源码，解压之后进入目录，新建build目录，在build目录下执行
```
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8
sudo make install
```

## 编译

在blimp-autopilot下执行`catkin_make -DCMAKE_BUILD_TYPE=Release`即可执行编译

- 不加`-DCMAKE_BUILD_TYPE=Release`也可以执行编译，但运行时性能较差
- 根据实际情况，添加`-j2`或`-j8`等选项，提升编译速度；若不指定，默认等于CPU核心数目

# 硬件连接
首先使用串口连接昇腾底板，波特率115200，使用MobaXterm等工具进行连接。开机过程中需要等待一段时间，待出现登录提示时，输入账户和密码登录Ubuntu 22.04系统。

登录之后，运行`./wifi_XXX.sh`以连接无线网络。若连接成功，将显示其IP地址等信息；若连接失败，可以重新运行`./wifi_XXX.sh`；若连续失败，可以拔插无线网卡模块并重新运行`./wifi_XXX.sh`，直至连接成功为止。

在无线网络连接成功后，确保自己的计算机与昇腾连接在同一网络内，使用Ping等方式确认连通性没有问题后，使用MobaXterm或VSCode等工具登录昇腾，连接没有问题后即可断开串口连接。

# 环境变量及配置

## 环境变量

* roscore的环境变量
* mavros的环境变量
* realsense的环境变量
* VINS的环境变量
* ego_planner的环境变量
* odometry_visualization的环境变量（在本地）

可以通过在新启动的终端中source相关的devel/setup.bash来实现，也可以写入.bashrc来实现

## 多机版ROS需要配置IP

主机侧与从机侧均需要在.bashrc里配置`ROS_MASTER_URI`与`ROS_IP`两个变量，其中
1. 所有的`ROS_MASTER_URI`均须与主机的IP保持一致
2. 所有的`ROS_IP`均须与自身的IP保持一致

# 使用ROSBAG进行离线验证

## 准备工作

如果手头没有完善的硬件设备，可以使用ROSBAG在本地计算机上运行VINS-Fusion与Ego-planner，验证代码编译是否有问题。

如果需要运行VINS-Fusion，需要的rosbag中至少要包括（以双目相机+IMU的配置为例）：
- 左目相机的topic，用于vins定位，在我们的例子中其名称为/camera/infra1/image_rect_raw
- 右目相机的topic，用于vins定位，在我们的例子中其名称为/camera/infra1/image_rect_raw
- IMU的topic，用于vins定位，在我们的例子中其名称为/mavros/imu/data或/mavros/imu/data_raw

而如果需要运行Ego-planner，需要的rosbag中至少包括：
- 里程计的topic，用于轨迹规划，在我们的例子中其名称为/loop_fusion/odometry_rect
- 深度相机的topic，用于建图与避障，在我们的例子中其名称为/camera/depth/image_rect_raw

如果同时运行Vins-Fusion与Ego-planner，则可以省去里程计的topic，因为VINS-Fusion会输出这个topic
- 左目相机的topic，用于vins定位，在我们的例子中其名称为/camera/infra1/image_rect_raw
- 右目相机的topic，用于vins定位，在我们的例子中其名称为/camera/infra1/image_rect_raw
- IMU的topic，用于vins定位，在我们的例子中其名称为/mavros/imu/data或/mavros/imu/data_raw
- 深度相机的topic，用于建图与避障，在我们的例子中其名称为/camera/depth/image_rect_raw

运行之前需要确保stereo_imu_config_d435f.yaml中标注的各项topic与rosbag中各个topic能对应上。

## 离线运行

（首先应保证所有的环境变量已经配置正确，可以在每个新开的终端中source相关的setup.bash，也可以写入~/.bashrc中）

1. 开启一个终端，启动roscore
2. 开启一个终端，进入blimp-autopilot/shfiles，将vins_run.sh中关于启动realsense的语句注释或删去`roslaunch realsense2_camera rs_camera.launch & sleep 6;`，并在这个终端中运行`./vins_run.sh`，并等待至终端输出“no previous graph”
3. 开启一个终端，播放rosbag：`rosbag play rosbag_demo.bag`，这时将可以看到启动`vins_run.sh`的终端中会有新的输出
4. 开启新的终端，可以使用`rostopic echo /loop_fusion/odometry_rect`等指令观察里程计输出，也可以`roslaunch odom_visualization vins_vis.launch`来进行可视化的观察
5. 开启新终端，在确保里程计的topic已经发布之后，在blimp-autopilot/shfiles中执行`./ego_plan.sh`，等待出现绿色文字提示规划成功且不断刷新，也可以在可视化窗口中观察到规划的轨迹

# 运行VINS

## 理论上的执行顺序

1. 配置mavros环境变量，并启动mavros节点
2. 配置realsense环境变量，并启动realsense节点
3. 配置vins相关的环境变量，并依次启动vins_node与loop_fusion_node

## 实际上的运行方式
首先进入/root/blimp-autopilot-master/shfiles/ascend/路径下，然后开两个终端分别执行以下操作
1. ./mavros.sh
2. ./vins_run.sh

然后即可使用`rostopic echo /loop_fusion/odometry_rect`来查看里程计数据。

具体来看，这两个脚本分别实现了以下功能：

mavros.sh（需要管理员权限）
```
source ../devel/setup.bash
sudo chmod 666 /dev/ttyACM0
roslaunch mavros px4.launch
```

vins_run.sh
```
source ../devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

rosrun vins vins_node /root/blimp-autopilot-master/src/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml & sleep 5;

rosrun loop_fusion loop_fusion_node /root/blimp-autopilot-master/src/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml & sleep 1;
```
在启动了VINS之后，得到了视觉里程计输出，即可使用遥控器实现定点飞行等功能。

# 启动规划

先向下位机发送解锁以及定高指令
take_off.sh
```
source ../devel/setup.bash

rosservice call /mavros/set_mode "base_mode: 0
custom_mode: 'OFFBOARD'"

sleep 5

rosservice call /mavros/set_mode "base_mode: 0
custom_mode: 'AUTO.TAKEOFF'"
```

然后须使用指令`roslaunch ego_planner run_in_exp_435f.launch`来启动ego_planner节点；
待其启动之后，发送一条`/traj_start_trigger`消息来触发规划的进行，这可以通过ros pub指令来实现
```
rostopic pub -1 /traj_start_trigger geometry_msgs/PoseStamped "header:
    seq: 0
    stamp:
        secs: 0
        nsecs: 0
    frame_id: ''
pose:
    position:
        x: 0.0
        y: 0.0
        z: 0.0
    orientation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 0.0"
```
这两步可以合并至一个脚本中实现

ego_plan.sh
```
source ../devel/setup.bash;

roslaunch ego_planner run_in_exp_435f.launch & sleep 8;
./trigger.sh & sleep 1;
```




# 标定外参

修改stereo_imu_config_d435f.yaml中`estimate_extrinsic`为1，即“在线标定外参”，并将`print_extrinsic`设为1，即“在线输出外参”，与此同时，手动给出粗略的外参，至少保定平移变量的符号正确、绝对值大致接近真值。

启动vins并观察其输出`/loop_fusion/odometry_rect`或`/vins_estimator/odometry`，确认进程正常启动。拿起机体进行平缓的、充分的运动，持续观察里程计输出以及外参输出。当观察到外参达到收敛（变化不大）且里程计较为准确（至少没有发散），即可停止vins的运行，并将最新的外参值写入stereo_imu_config_d435f.yaml的对应位置
```
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [-0.019802,0.096046,0.995180,0.138254,
          -0.998751,0.043769,-0.024097,0.033161,
          -0.045873,-0.994414,0.095059,-0.040689,
          0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [-0.021758,0.096712,0.995075,0.138524,
          -0.998741,0.042888,-0.026006,-0.011314,
          -0.045192,-0.994388,0.095657,-0.042557,
          0., 0., 0., 1. ]
```
最后将`estimate_extrinsic`和`print_extrinsic`均改为0，须注意外参矩阵的格式，每一行的开头均需对齐。

# 常见的可调节参数及其效果

## VINS相关（视觉自主定位）

### 相机帧率

改变相机的原始输出帧率需要通过修改realsense包的launch文件来实现。
/src/vins-fusion/realsense-ros-ros1-legacy/realsense2_camera/launch/rs_camera.launch中的depth_fps与infra_fps分别对应了深度图与灰度图的帧率。

- 当帧率过低，VINS的输出帧率也会跟着降低且不够平滑；
- 当帧率过高，VINS的计算负担过重，可能导致实时性变差、待求解数据在队列里堆积（内存耗尽）、求解失败（里程计发散）等问题。


### 外参

外参写在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中，是左右目相机在IMU坐标系下的齐次变换矩阵。外参的准确与否很大程度上决定了里程计的准确程度，甚至会影响里程计是否发散。参考*外参标定*。

### mod_num

此参数同样写在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中，是一个用于对相机原始图像帧率进行降采样的参数，里程计输出的帧率等于相机原始帧率处以mod_num，可以在不改变相机原始帧率的情况下对里程计帧率进行修改。

### 多线程

在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中的multiple_thread为0时为单线程模式，特征点的提取与基于特征的相对位姿计算在同一线程内完成；当此值不为0时，特征点的提取与相对位姿解算则分成两个线程进行。

- 当未开启多线程时，前端的耗时为特征提取与位姿解算两者之和；
- 当开启多线程时， 前端的耗时为特征提取与位姿解算两者中较长的那个。

## ego-planner相关（自主避障规划）

### 膨胀半径

src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/obstacles_inflation

进行轨迹规划时，为了避免机体与障碍物的碰撞，需要对建立好的占据地图进行膨胀，膨胀半径不小于机体最小外接圆的半径。

- 此膨胀半径如果设置过小，则规划出的轨迹会与障碍物相距过近，机体飞行过程中可能与障碍物发生碰撞；
- 此半径如果设置过大，则留给机体的可行空间过小，有可能无法搜索到可行解，或通行效率降低。

### 球心高度

这个高度出现在以下几个位置：
- 在src/ego-planner/planner/plan_manage/src下traj_server.cpp中
    - assignment_cmd中对cmd_.position.z减去的高度
    - assignment_cmd_mavros中对cmd_mavros_.position.z减去的高度
- 在src/ego-planner/planner/plan_manage/src下ego_replan_fsm.cpp中
    - EGOReplanFSM::assignment_cmd中对msg.position.z减去的高度
    - EGOReplanFSM::assignment_cmd_mavros中对cmd_mavros.position.z减去的高度


球心高度是指IMU至气球机器人外接球球心的垂直高度，它应当与实际相符。

- 当球心高度比实际低，气球机器人可能碰到高处的障碍物（如天花板等）；
- 当球心高度比实际高，气球机器人可能碰到低处的障碍物（如地面或气球机器人试图越过的障碍物等）。

*若需要修改此参数，修改完需要重新编译ego-planner*

### 虚拟天花板与地板

src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/virtual_ceil为虚拟天花板，grid_map/virtual_ground为虚拟地板，这两个参数主要用于应对相机视野有限、向前拍摄时无法观察到真实的天花板与地板的问题。通过设置此虚拟天花板，在ego-planner建立的局部栅格占据地图中加入虚拟的天花板与地面，人为限制轨迹规划的可行空间，避免规划器向它看不到但实际有障碍的区域规划。

以虚拟天花板为例：
- 当天花板设置过高，比真实场景中的天花板更高，飞艇如果发现前方被障碍物占据，在看不到上方真实天花板的情况下，可能规划穿入天花板的轨迹来从上方越过障碍物，直到气球已经撞到天花板，由于其视野受限依然无法发现上方的天花板，会继续增大油门向上飞，可能发生危险；
- 当天花板设置过低，会严重限制规划器的可行空间，导致飞艇前方明明有宽阔的可行空间，但在ego-planner看来，前方已经被障碍物完全遮挡而无法规划出可行轨迹。

虚拟地面的情况与之类似。

### 速度、加速度、角速度限制

在src/ego-planner/src/planner/plan_manage/launch/run_in_exp_435f.launch中，max_acc与max_vel是规划轨迹时的线加速度与线速度上限，而src/ego-planner/src/planner/plan_manage/src/ego_replan_fsm.cpp中的max_yaw_dot是气球机器人转动时的角速度上限。

### 目标点设定

在在src/ego-planner/planner/plan_manage/launch/run_in_exp_435f.launch中，point_num代表了一共有几个导航点（规划开始前气球机器人所处的当前位置不计入总导航点数内），然后在下方由0开始编号的即为规划要考虑的导航点，若point_num为3，则只有point0、point1与point2起作用。目前最多支持5个导航点，要添加更多导航点须对源码中其他部分进行对应修改，可以参见ego-planner-v2开源代码仓中的说明。

# 常见错误与排除方式

## VINS启动了，但没有里程计输出

* 没有IMU数据
观察启动VINS的终端里是否持续输出`wait for imu...`，如果是，则使用`rostopic hz /mavros/imu/data`判断是否有IMU数据以及其帧率是否为200Hz
1. 若发现没有输出，则须排查mavros节点是否成功启动了
2. 若有输出，但帧率不对，如帧率为10Hz等，须断电重启，听电调声音判断飞控是否正常开机，如果不起作用，则需要使用QGround地面站软件进一步排除是否为sysconfig.toml文件配置问题

* 没有图像数据
如果IMU数据正常，启动VINS的终端也没有持续输出`wait for imu...`，检查realsense是否正常输出图像，使用`rostopic hz /mavros/camera/infra1/image_rect_raw`来检查图像帧率是否为15Hz
1. 如果帧率为0，说明没有读取到图像数据，检查realsense节点是否启动成功，重启此节点并再次尝试，`roslaunch realsense2_camera rs_camera.launch`
2. 如果帧率错误，则检查realsense节点的launch文件是否有问题，具体路径为`VINS_SRC_PATH/realsense-ros-ros1-legacy/realsense2_camera/launch/rs_camera.launch`

## 无法启动mavros或realsense，甚至roscore都无法启动

* 检查多机版ROS配置是否正确
1. 在昇腾端使用`ifconfig`查看IP地址，查看.bashrc中的`ROS_MASTER_URI`与`ROS_IP`是否与之一致。

* 检查ROS相关环境变量是否正确
1. 使用`echo $ROS_PACKAGE_PATH`检查ROS路径，查看所要运行的包的路径是否被包含在其中

* 检查conda配置中是否启用了base环境
1. 若运行roscore时提示`rosversion roslaunch`相关的错误，观察一下终端每一行命令的最前面是否有`(base)`，若没有，则输入`conda activate base`进行尝试

## VINS发散

造成VINS发散的原因有很多，有些是使用不当或程序配置不当导致的，属于工程问题，可以通过工程手段解决；而另一些是使用场景及VINS本身性能极限所导致的，这是原理方面的，一般只能尽量规避。

* 如果VINS一启动立刻就发散，检查其帧率，降低帧率进行尝试，可以降低realsense的原始图像帧率，也可以修改`stereo_imu_config_d435f.yaml`中的`mod_num`
* 如果原地不动VINS不发散，但一开始运动就发散，检查IMU是否装反（或相机朝向是否反了）
* 如果平移不发散，而一旋转就发散，或旋转不发散，而一平移就发散，则使用`rostopic echo /mavros/imu/data`观察IMU数据是否正常（是否包含线加速度、姿态、角速度完整的部分，其符号是否正确、读数范围是否大致正确）
* 保持`estimate_extrinsic`为1的情况下，VINS会持续对外参进行估计，但相当于在一个优化问题中引入了更多的自由变量，在不良工况下发散的可能性会更大一些
* 机体转动过快时更容易发散，因为快速的转动比快速平移时画面中特征点的位移更大、画面的动态模糊更严重，此时光流追踪更容易失败
* 当摄像头朝向缺乏纹理的白墙、容易反光的玻璃/镜子、具有大量重复纹理的墙纸/地砖或场景中存在大量运动物体时，VINS较容易发散

## Ego-Planner自主运动中可能出现的问题

### 无法规划可行轨迹

* 启动ego-planner后，在可视化界面中发现前方几乎全是障碍物，将可行空间堵死而无法规划出可行轨迹
1. 检查膨胀半径是否过大
2. 检查虚拟天花板设置是否过低

* 前方有可行空间，ego-planner一直在尝试求解但始终无法得到可行解
1. 检查所设置的目标点是否位于障碍物区域内，如果目标点被障碍物（膨胀之后的）包裹，则无论如何也无法规划出不碰撞的可行轨迹

### 飞行轨迹不合理

* 左右侧有可行空间，但飞艇直接向上飞，甚至撞到天花板
1. 检查虚拟天花板是否过高，无法起到限制作用

* 飞艇在飞行过程中，轨迹超调严重，轨迹跟踪精度不足
1. 检查速度、加速度上限是否设置过高；
2. 检查飞艇重量，“飞艇自身+气囊升力+配重”全部抵消之后的净重力维持在10～20g为最佳状态，净重过大会严重影响控制性能，可以通过称重判断是否存在此问题，也可以通过飞艇原地悬停时螺旋桨转速来判断。