深入解读Fast-Planner算法看这一篇就够！（含Ubuntu20.04 + Ros noetic 环境下 Fast-planner 算法仿真环境的配置。）

前言
Fast-Planner的论文由香港科技大学相关团队发表于2019年，其快速穿越森林的应用场景令人惊叹，Fast-Planner算法需要全局地图信息，其核心是“快速矩阵式A*”（Fast Matrix-Style A*）的搜索算法，可以类比为混合A*算法，区别在于其构建的代价函数有所不同，这是对无人机路径规划领域极大的贡献。以下将分为三大部分（论文解读、仿真环境配置、算法真机测试）介绍Fast-Planner算法。希望对大家有所帮助！

Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight | IEEE Journals & Magazine | IEEE Xplore
https://ieeexplore.ieee.org/document/8758904

GitHub - HKUST-Aerial-Robotics/Fast-Planner: A Robust and Efficient Trajectory Planner for Quadrotors
https://github.com/HKUST-Aerial-Robotics/Fast-Planner

论文解读
笔者将解读流程记在了语雀中，此处就不再赘述了，具体点击即可链接查看：

https://www.yuque.com/adamas-tavq8/mxxg0h/dv0qz41m9eu177cb
https://www.yuque.com/adamas-tavq8/mxxg0h/dv0qz41m9eu177cb

算法仿真
仿真环境：Ubuntu20.04 + ROS noeric + g++ 9.4.0 + Eigen 3.3.7

1. 下载源码
mkdir -p competition_ws/src
cd competition_ws/src
catkin_init_workspace
git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git
2. 安装库与相关包
第一种方法是用apt软件仓库输入命令sudo apt install ros-noetic-nlopt直接安装
第二种方法是下载源码用cmake安装。
（eg：如果是ubuntu16.04或者ubuntu18.04可以直接从apt安装，比源码安装方便。但是ubuntu20.04用第一种方法安装会报错用不了，（不信可以试试）所以得用源码安装）

sudo apt install libarmadillo-dev //安装线性库
 
git clone https://github.com/stevengj/nlopt.git//非线性优化库安装
cd nlopt
mkdir build && cd build
cmake ..
make
sudo make install
（eg：若配置的仿真环境为ubuntu18.04 + ROS melodic，可以直接采用catkin make或者catkin build 编译文件并运行即可，但在ubuntu80.04 + ROS noetic环境下会出现以下问题，跟着笔者一步步解决即可）

3. 编译过程问题解答 
3.1.1 使用catkin_make指令编译
（eg：若采用catkin build指令编译则浏览3.1.2）

问题1：pcl包无法找到：

解决1：指定编译的c++版本标准为c++14，在每一个CMakeLists.txt文件中在projrct后面加入以下代码：

set(CMAKE_CXX_STANDARD 14)
问题2：nlopt包找不到（如下图所示）

 解决2：由于bspline_opt中的CMakeLists.txt找的是ros-noetic-nlopt，所以需要修改一下bspline_opt中的CMakeLists.txt，找到用源码安装的nlopt，修改后的CMakeLists.txt文件如下所示：

cmake_minimum_required(VERSION 2.8.3)
project(bspline_opt)
find_package(NLopt REQUIRED)
set(NLopt_INCLUDE_DIRS ${NLOPT_INCLUDE_DIR})
 
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
 
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
visualization_msgs
cv_bridge
plan_env
)
 
catkin_package(
INCLUDE_DIRS include
LIBRARIES bspline_opt
CATKIN_DEPENDS plan_env
#  DEPENDS system_lib
)
 
include_directories( 
SYSTEM 
include 
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS} 
${PCL_INCLUDE_DIRS}
${NLOPT_INCLUDE_DIR}
)
 
set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS} -O3 -Wall")
 
add_library( bspline_opt 
src/bspline_optimizer.cpp 
)
target_link_libraries( bspline_opt
${catkin_LIBRARIES} 
${NLOPT_LIBRARIES}
)  

3.1.2 采用catkin build编译
由于catkin build是多线程同时编译，catkin_make按照顺序进行编译，导致出现了以下问题：

据本人分析，此问题是因为头文件所在的文件夹由于不是按顺序编译导致没有先生成头文件就先调用，所以会报错，所以要先编译需要调用的文件所在的功能包，可以先编译指定功能包：

catkin build multi_map_server
单独编译此文件夹时，又出现了以下错误：

分析可能由两个原因导致找不到功能包中写好的包：

第一个原因是功能包中不存在此文件
第二个原因是没有把对应的.so文件安装到系统
（eg：此处采用第一种方法将libpose_utils.so文件复制到/usr/local/lib路径下，当然也可以修改CMakeLists.txt文件，使其在编译时就直接完成安装，此时即可成功编译）

catkin build multi_map_server
catkin build
3.2 运行程序时可能发生的问题
问题1：运行程序时出现以下报错

        解决1：到/Fast-Planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp中把/world前面的/去掉，ubuntu20.04对/敏感。改完编译运行后会看到小飞机出来了，但是还出现了问题2.

问题2：函数返回值错误，报错#6如下：

        解决2：到/Fast-Planner/fast_planner/path_searching/src/kinodynamic_astar.cpp里搜KinodynamicAstar::timeToIndex函数，可以看到函数返回值是int，但是函数体没有return，修改完如下：

int KinodynamicAstar::timeToIndex(double time)
{
    int idx = floor((time - time_origin_) * inv_time_resolution_);
    return idx;
}
        到Fast-Planner/fast_planner/plan_env/include/edt_environment.h第69和71行，把返回值类型改成void，再去cpp文件中84到118行做同样修改，修改后的代码如下：

void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d& diff,
                                                     double& value, Eigen::Vector3d& grad);
void evaluateEDTWithGrad(const Eigen::Vector3d& pos, double time,
                                                    double& dist, Eigen::Vector3d& grad);
void EDTEnvironment::interpolateTrilinear(double values[2][2][2],
                                                                   const Eigen::Vector3d& diff,
                                                                   double& value,
                                                                   Eigen::Vector3d& grad) {
  // trilinear interpolation
  double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
  double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
  double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
  double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
  double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
  double v1 = (1 - diff(1)) * v01 + diff(1) * v11;
 
  value = (1 - diff(2)) * v0 + diff(2) * v1;
 
  grad[2] = (v1 - v0) * resolution_inv_;
  grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * resolution_inv_;
  grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
  grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
  grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
  grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
  grad[0] *= resolution_inv_;
}
 
void EDTEnvironment::evaluateEDTWithGrad(const Eigen::Vector3d& pos,
                                                                  double time, double& dist,
                                                                  Eigen::Vector3d& grad) {
  Eigen::Vector3d diff;
  Eigen::Vector3d sur_pts[2][2][2];
  sdf_map_->getSurroundPts(pos, sur_pts, diff);
 
  double dists[2][2][2];
  getSurroundDistance(sur_pts, dists);
 
  interpolateTrilinear(dists, diff, dist, grad);
}

完成上述步骤后运行以下指令即可正常运行仿真：

source devel/setup.bash
roslaunch plan_manage rviz.launch
roslaunch plan_manage kino_replan.launch

算法真机测试
首先在Jetson Xavier NX 中配置好Ubuntu20.04 + ROS noeric + g++ 9.4.0 + Eigen 3.3.7运行环境，其下载编译流程与仿真环境中的编译大同小异，但是由于Jetson Xavier NX为arm架构，会出现一些问题：

1. 编译问题
问题1：动态库版本错误： 

        解决1：由于libpose_utils.so是已经在x86_64中已经编译好的库文件，所以不能够直接在arm架构中直接使用，所以找到pose_utils文件夹，在arm中直接编译得到.so文件，然后再编译文件夹中的devel/lib中找到编译好的.so文件直接替代掉原有文件，然后复制一份到usr/lib中再次编译即可。

问题2：真机运行时，规划过程中的高度一直维持在1米：
        解决2：修改keno_replan_fsm.cpp文件中的第74行，更改为：

end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, msg->poses[0].pose.position.z;
        并将launch文件中的参数改为1：

 <arg name="flight_type" value="1" />
2. 程序话题修改（以比赛为例子）
实际上Fast-Planner订阅了三个话题，并发布了一个位置控制话题，需要调用Fast-Planner时我们仅需要修改相应的话题即可。

2.1 修改订阅话题
（eg：一定要确保消息类型相同，不同时要转换）

订阅无人机位置话题：/state_ukf/odom
        消息类型：nav_msgs/Odometry

        修改为：/iris_0/mavros/odometry/in（以笔者的里程计话题为例）

订阅无人机的相机深度信息：/pcl_render_node/depth
        消息类型：sensor_msgs/Image

        修改为：/iris_0/realsense/depth_camera/depth/image_raw（以笔者深度相机话题为例）

订阅无人机相机姿态话题：/pcl_render_node/camera_pose
        消息类型：geometry_msgs/PoseStamped

        修改为：/iris_0/mavros/vision_pose/pose（此处以笔者的相机姿态为例）

2.2 修改发布话题
（eg：一定要确保消息类型相同，不同时要转换）

发布无人机的位置控制话题：/planning/pos_cmd
        消息类型：quadrotor_msgs/PositionCommand

        修改为：/pose_cmd

        同时在traj_server.cpp文件中添加以下代码发布控制指令

ros::Publisher pose_cmd_pub;
geometry_msgs::Pose pose_cmd;
 
  pose_cmd.position.x = pos(0);
  pose_cmd.position.y = pos(1);
  pose_cmd.position.z = pos(2);
 
  pose_cmd.orientation.x = 0.0; 
  pose_cmd.orientation.y = 0.0;
  pose_cmd.orientation.z = sin(yaw/2);
  pose_cmd.orientation.w = cos(yaw/2);
  //发送一个geometry_msgs::Pose类型的消息
  pose_cmd_pub.publish(pose_cmd);
  
  pose_cmd_pub = nh.advertise<geometry_msgs::Pose>("/pose_cmd", 50);
3. 程序启动步骤
（1）将无人机起飞到一定高度，切换为off board模式

（2）启动fast_palnner节点：

roslaunch plan_manage kino_replan.launch
（3）发布目标点话题即可