teb_local_planner讲解

1　如何安装

1.1　简单方法

　　最省事的方法是用下面的指令安装，但缺点是你没法修改程序，因为安装的是编译后的二进制文件。

sudo apt-get install ros-kinetic-teb-local-planner
sudo apt-get install ros-kinetic-teb-local-planner-tutorials

　　安装路径在：/opt/ros/kinetic/share文件夹下。这个路径下你不能直接修改文件，要先用sudo chmod -R 777 *修改权限。能看到里面没有源代码文件。

　　如果你曾经删除过一些库（比如PCL），可能会连带删掉一些插件，会导致报错，这时重新安装即可，例如报缺少全局规划器的错，那就用以下指令安装。缺什么装什么，所有可能缺的在这里。

sudo apt-get install ros-kinetic-global-planner

1.2　源码安装

　　如果你想修改或者优化Teb的程序，可以从源代码安装，源代码地址点这里。安装过程参考这篇博客。安装完以后执行source devel/setup.bash，然后再运行rospack plugins --attrib=plugin nav_core就能使用这个编译后的程序了。即使通过指令安装的ROS的Teb程序依然存在，但是此时的默认的Teb局部规划器执行路径已经改为你安装的程序路径了，这个从下图中可以看出：

　　运行以下命令就能打开它自带的一个简单的测试例子：

roslaunch teb_local_planner test_optim_node.launch

　　但是你可能会犯一些错误。如果出现以下错误，说明你没有source，你应该在安装目录下运行source devel/setup.bash。

q@ubuntu:~$ rosdep install teb_local_planner
ERROR: Rosdep cannot find all required resources to answer your query
Missing resource teb_local_planner
ROS path [0]=/opt/ros/kinetic/share/ros

　　下载源代码的时候一定注意版本，如果你的ROS是kinetic版，就要切换分支（因为默认的是melodic版）。

　　否则会报很多错：

error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> >::BlockSolver(std::remove_reference<std::unique_ptr<g2o::LinearSolverCSparse<Eigen::Matrix<double, -1, -1> > >&>::type)’

　　切换分支的方法如下：例如下载kinetic版的命令是（用git clone -b kinetic-devel）：

git clone -b kinetic-devel https://github.com/rst-tu-dortmund/teb_local_planner.git

　　如果你不小心已经安装了ROS官方的Teb，那么卸载Teb的步骤是：先用apt-cache search ros-kinetic查询这个包完整的名字（有点长，如果你不记得的话），如下图，名字是ros-kinetic-teb-local-planner（还有例子ros-kinetic-teb-local-planner-tutorials）。然后用sudo apt-get remove ros-kinetic-teb-local-planner删除这个包即可。最后用rospack list检查一下，发现找不到teb了就说明删除成功。

　　我从源代码安装完teb_local_planner_tutorials例子后运行时报错：大意是teb_local_planner没有注册为局部路径规划器。但是我们已经把所有依赖的库和包都安装了，没有理由报错了啊。无奈我又从ROS的官方源（official repositories）安装了一遍：sudo apt-get install ros-kinetic-teb-local-planner-tutorials。这样终于可以运行了，我可以修改teb的源代码然后用例子进行检验效果了，我的目的终于实现了。

Failed to create the teb_local_planner/TebLocalPlannerROS planner, are you sure it is properly registered and that the containing library is built? Exception: According to the loaded plugin descriptions the class teb_local_planner/TebLocalPlannerROS with base class type nav_core::BaseLocalPlanner does not exist. Declared types are  base_local_planner/TrajectoryPlannerROS dwa_local_planner/DWAPlannerROS pose_follower/PoseFollower

2　特点分析

　　Teb可以实现静态/动态障碍物、通过给定点、最小转向半径等多种约束条件下的时间最优轨迹规划，具备这样优秀特性的规划方法实在不多，更难能可贵的是它还给出了详细的例子和规范的代码。以至于任何想做无人驾驶的同行都会第一个想起它。但是它的缺点也是比较严重的，在使用过程中笔者发现了很多问题：
　　(1)　控制量不平滑，尤其是前轮的转角上蹿下跳。直接用在无人车上简直是灾难，你会看到前轮像疯了似的左右摆动，甚至能把电机搞坏。画出前轮角度的图像如下图中的蓝色曲线，可见质量极差。

<node name="rqt_plot"  pkg="rqt_plot"  type="rqt_plot" args="/cmd_vel/angular/z" />

　　(2)　解的质量不稳定，因为采用了数值优化的方法计算轨迹，所以对初值敏感。同样的起点和终点，不同的初值会得到不同的轨迹。轨迹有时候比较好，但你不知道什么时候质量会比较差（看起来很怪异，各种倒车）。