Ubuntu 20.04 + FastPlanner

下载源码

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

依赖

C++线性代数库

sudo apt install libarmadillo-dev

非线性优化库nlopt

git clone https://github.com/stevengj/nlopt.git
cd nlopt
mkdir build && cd build
cmake ..
make
sudo make install

// 注意：

1. 如果是ubuntu16.04 ubuntu18.04，那么一行命令就ok:

   sudo apt install ros-noetic-nlopt
   

2. 须在fast planner的bspline_opt的CMakeLists.txt设置寻找源码安装的nlopt：

   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}
   )
   

更改C++版本

1. 每个CMakeLists.txt里把C++11改成C++14

更改函数返回值

1. /FastPlanner/fast_planner/path_searching/src/kinodynamic_astar.cpp的函数
   KinodynamicAstar::timeToIndex

   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

   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);
   }