TEB算法-debug

a. 编译bug记录

下面记录我在学习整个项目时遇到的bug问题。学习的项目不是原本的teb官方算法，而是另一位作者的非ROS实现。GitHub：https://github.com/linyicheng1/teb_local_planner

无法匹配到函数

我并没有完全编译好整个项目，因为我编译了16年的g2o，然后再编译项目作者的代码，还是不能通过。

原因分析：大概是g2o已经更改了一些函数的接口，上网看了下出现的原因，也确实是因为g2o为了安全原因，改了一些函数。https://github.com/RainerKuemmerle/g2o/issues/206

依旧有以下bug：不能找到对应的函数。

解决办法：修改相应的参数，进行类型转换。

1. src/optimal_planner.cpp:121:118

   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:121:118: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::remove_reference<std::unique_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> > >&>::type)’
            g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
            
   # 下面是提示出错的原因
   In file included from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/../inc/optimal_planner.h:21:0,
                    from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:1:
   /usr/local/include/g2o/core/optimization_algorithm_levenberg.h:45:16: note: candidate: g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(g2o::Solver*)
          explicit OptimizationAlgorithmLevenberg(Solver* solver);
   /usr/local/include/g2o/core/optimization_algorithm_levenberg.h:45:16: note:   no known conversion for argument 1 from ‘std::remove_reference<std::unique_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> > >&>::type {aka std::unique_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> > >}’ to ‘g2o::Solver*’
   

2. src/optimal_planner.cpp:120:97

   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:120:97: 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)’
            std::unique_ptr<TEBBlockSolver> block_solver(new TEBBlockSolver(std::move(linear_solver)));
            
   # 下面是提示出错的原因
   In file included from /usr/local/include/g2o/core/block_solver.h:189:0,
                    from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/../inc/optimal_planner.h:18,
                    from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:1:
   /usr/local/include/g2o/core/block_solver.hpp:39:1: note: candidate: g2o::BlockSolver<Traits>::BlockSolver(g2o::BlockSolver<Traits>::LinearSolverType*) [with Traits = g2o::BlockSolverTraits<-1, -1>; g2o::BlockSolver<Traits>::LinearSolverType = g2o::LinearSolver<Eigen::Matrix<double, -1, -1> >]
    BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
    ^~~~~~~~~~~~~~~~~~~
   /usr/local/include/g2o/core/block_solver.hpp:39:1: note:   no known conversion for argument 1 from ‘std::remove_reference<std::unique_ptr<g2o::LinearSolverCSparse<Eigen::Matrix<double, -1, -1> > >&>::type {aka std::unique_ptr<g2o::LinearSolverCSparse<Eigen::Matrix<double, -1, -1> > >}’ to ‘g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> >::LinearSolverType* {aka g2o::LinearSolver<Eigen::Matrix<double, -1, -1> >*}’
   

3. src/optimal_planner.cpp:84行-103行：其中一个例子

   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:103:120: error: no matching function for call to ‘g2o::Factory::registerType(const char [19], std::shared_ptr<g2o::HyperGraphElementCreator<teb_local_planner::EdgePreferRotDir> >)’
            factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
   
   # 下面为提示出错的原因
   In file included from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/../inc/optimal_planner.h:19:0,
                    from /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:1:
   /usr/local/include/g2o/core/factory.h:62:12: note: candidate: void g2o::Factory::registerType(const string&, g2o::AbstractHyperGraphElementCreator*)
          void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c);
               ^~~~~~~~~~~~
   /usr/local/include/g2o/core/factory.h:62:12: note:   no known conversion for argument 2 from ‘std::shared_ptr<g2o::HyperGraphElementCreator<teb_local_planner::EdgePreferRotDir> >’ to ‘g2o::AbstractHyperGraphElementCreator*’
   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp: In member function ‘boost::shared_ptr<g2o::SparseOptimizer> teb_local_planner::TebOptimalPlanner::initOptimizer()’:
   

解决办法

1. 对于以下报错，需要通过检查CMakeList.txt中的依赖，和官网上写的依赖，确定无误后，需要删除以前安装的g2o，再编译，再安装。删除脚本放在报错下面。

   # 报错1：
   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:121:118: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::remove_reference<std::unique_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1> > >&>::type)’
            g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
   
   # 报错2：
   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:120:97: 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)’
            std::unique_ptr<TEBBlockSolver> block_solver(new TEBBlockSolver(std::move(linear_solver)));
   

   删除脚本：

   sudo rm -rf /usr/local/include/g2o
   sudo rm /usr/local/lib/libg2o_*.so
   find /usr/local/lib -name "*libg2o*.so" -type f # 查看是否删除干净
   
   sudo rm /usr/local/bin/g2o*
   find /usr/local/bin -name "*g2o*" -type f # 查看是否删除干净
   

2. 对于以下报错，需要修改源代码，即可解决

   /home/cislc2019/code/loyer/navigation-LidarAndVisual/teb_local_planner/src/optimal_planner.cpp:103:120: error: no matching function for call to ‘g2o::Factory::registerType(const char [19], std::shared_ptr<g2o::HyperGraphElementCreator<teb_local_planner::EdgePreferRotDir> >)’
            factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
   

   原本的源代码为：

   factory->registerType("VERTEX_POSE", std::make_shared<g2o::HyperGraphElementCreator<VertexPose>>());
   factory->registerType("VERTEX_TIMEDIFF", std::make_shared<g2o::HyperGraphElementCreator<VertexTimeDiff>>());
   
   factory->registerType("EDGE_TIME_OPTIMAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeTimeOptimal>>());
   factory->registerType("EDGE_SHORTEST_PATH", std::make_shared<g2o::HyperGraphElementCreator<EdgeShortestPath>>());
   factory->registerType("EDGE_VELOCITY", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocity>>());
   factory->registerType("EDGE_VELOCITY_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>>());
   factory->registerType("EDGE_ACCELERATION", std::make_shared<g2o::HyperGraphElementCreator<EdgeAcceleration>>());
   factory->registerType("EDGE_ACCELERATION_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationStart>>());
   factory->registerType("EDGE_ACCELERATION_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationGoal>>());
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>>());
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>>());
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", std::make_shared<g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>>());
   factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>>());
   factory->registerType("EDGE_KINEMATICS_CARLIKE", std::make_shared<g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>>());
   factory->registerType("EDGE_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeObstacle>>());
   factory->registerType("EDGE_INFLATED_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeInflatedObstacle>>());
   factory->registerType("EDGE_DYNAMIC_OBSTACLE", std::make_shared<g2o::HyperGraphElementCreator<EdgeDynamicObstacle>>());
   factory->registerType("EDGE_VIA_POINT", std::make_shared<g2o::HyperGraphElementCreator<EdgeViaPoint>>());
   factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared<g2o::HyperGraphElementCreator<EdgePreferRotDir>>());
   

   修改为：

   factory->registerType("VERTEX_POSE", new g2o::HyperGraphElementCreator<VertexPose>);
   factory->registerType("VERTEX_TIMEDIFF", new g2o::HyperGraphElementCreator<VertexTimeDiff>);
   
   factory->registerType("EDGE_TIME_OPTIMAL", new g2o::HyperGraphElementCreator<EdgeTimeOptimal>);
   factory->registerType("EDGE_SHORTEST_PATH", new g2o::HyperGraphElementCreator<EdgeShortestPath>);
   factory->registerType("EDGE_VELOCITY", new g2o::HyperGraphElementCreator<EdgeVelocity>);
   factory->registerType("EDGE_VELOCITY_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeVelocityHolonomic>);
   factory->registerType("EDGE_ACCELERATION", new g2o::HyperGraphElementCreator<EdgeAcceleration>);
   factory->registerType("EDGE_ACCELERATION_START", new g2o::HyperGraphElementCreator<EdgeAccelerationStart>);
   factory->registerType("EDGE_ACCELERATION_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationGoal>);
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomic>);
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicStart>);
   factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator<EdgeAccelerationHolonomicGoal>);
   factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator<EdgeKinematicsDiffDrive>);
   factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator<EdgeKinematicsCarlike>);
   factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeObstacle>);
   factory->registerType("EDGE_INFLATED_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeInflatedObstacle>);
   factory->registerType("EDGE_DYNAMIC_OBSTACLE", new g2o::HyperGraphElementCreator<EdgeDynamicObstacle>);
   factory->registerType("EDGE_VIA_POINT", new g2o::HyperGraphElementCreator<EdgeViaPoint>);
   factory->registerType("EDGE_PREFER_ROTDIR", new g2o::HyperGraphElementCreator<EdgePreferRotDir>);