Ubuntu20.04 运行Fast-Planner

运行环境：
ubuntu20.04 + ros-noetic
g++ 9.4
Eigen3.3.9

源码下载编译

1 创建工作空间克隆源码

mkdir -p fast_ws/src
cd fast_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git
cd ..

注：用git clone需要在自带终端，在terminator克隆不下来。

2 安装两个包

1 安装C++线性代数库
sudo apt install libarmadillo-dev
2 安装非线性优化库
这个有两种方法安装，第一种方法是用apt软件仓库输入命令sudo apt install ros-noetic-nlopt直接安装，第二种方法是下载源码用cmake安装。如果是ubuntu16.04或者ubuntu18.04可以直接从apt安装，比源码安装方便。但是ubuntu20.04用第一种方法安装会报错用不了，（不信可以试试）所以得用源码安装，README里也提到了这个问题，编译不报错但是生不出来轨迹。
安装过程也很简单，命令如下：

//先进入document目录，用源码下载的其他库都可以放在这
git clone https://github.com/stevengj/nlopt.git
cd nlopt
mkdir build && cd build
cmake ..
make
sudo make install

至此两个必要的安装包已经安装完成了。
接下来可以回到工作空间运行catkin_make命令进行编译了，也会报出一系列错误。

编译报错

1 C++14问题

没把C++11改成C++14之前编译会报很多类似上面的错，所以要去到每个CMakeLists.txt里把C++11改成C++14。

2 nlopt问题

由于bspline_opt中的CMakeLists.txt找的是ros-noetic-nlopt，没安装就会报下面这个错：

所以需要修改一下bspline_opt中的CMakeLists.txt，找到用源码安装的nlopt，这个可以按照README的提示修改，也可以直接复制我修改完的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}
)  

修改完之后再次编译，就可以编译成功了。

运行崩溃

安装README的提示运行，点击一个目标点后会报错。

1 运行rviz那个窗口报错/world问题

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

2 Segmentation fault

报错如下图所示：

你们可能有#6,没关系，这个需要改几个函数，这几个函数返回值有问题。
（1）到/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;
}

（2）到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);
}

修改完之后，再编译运行即可成功运行，如下图所示：