ROS从入门到精通5-4：路径规划插件开发案例(以A*算法为例)

1 路径规划问题

路径规划是移动机器人实现自主导航的关键技术之一。路径规划是指在有障碍物的环境中，按照一定的评价标准（如距离、时间、代价等），寻找到一条从起始点到目标点的无碰撞路径。

路径规划的核心是路径规划算法，常用的路径规划算法有：

• 人工势场法
• 模糊逻辑算法
• 栅格法——如Dijkstra算法、A算法、D算法、D* Lite 算法等
• 自由空间法
• 智能优化算法——如蚁群算法、遗传算法、神经网络算法等

2 ROS路径规划

本文不讨论具体的算法，只给出拿到一个具体的算法后如何部署到ROS系统中。

路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了move_base功能包，用于实现此功能。

move_base功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。

move_base主要由全局路径规划与本地路径规划组成。

3 自定义路径规划插件

参考ROS从入门到精通5-3：插件库与开发+实例分析开发插件。首先创建功能包my_planner用于生成自定义全局路径规划插件

1. 构造基类：由于全局路径规划插件全部继承于nav_core功能包的BaseGlobalPlanner类，因此无需构造

2. 构造插件类：在my_planner/include中新建my_planner.h，继承自基类nav_core::BaseGlobalPlanner，重点实现其中的initialize与makePlan接口

   namespace my_planner{
   
     class MyPlanner : public nav_core::BaseGlobalPlanner {
       public:
         MyPlanner();
         MyPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
         ~MyPlanner();
   
         void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
         void initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
   
         bool makePlan(const geometry_msgs::PoseStamped& start, 
             const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
     };
   };  
   

3. 注册插件：在my_planner/src中新建my_planner.cpp使用PLUGINLIB_EXPORT_CLASS宏注册插件，限于篇幅不列出完整代码，该代码完全复制官方carrot_planner作为测试用例。

   #include 
   #include 
   #include 
   #include 
   #include 
   #include 
   
   //register this planner as a BaseGlobalPlanner plugin
   PLUGINLIB_EXPORT_CLASS(my_planner::MyPlanner, nav_core::BaseGlobalPlanner)
   
   namespace my_planner {
     MyPlanner::MyPlanner()
     : costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){}
   
     void MyPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
       costmap_ros_ = costmap_ros;
       initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
     }
   
     void MyPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id){
       if(!initialized_){
   	...
       }
       else
         ROS_WARN("This planner has already been initialized... doing nothing");
     }
   
     bool MyPlanner::makePlan(const geometry_msgs::PoseStamped& start, 
         const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
   
       if(!initialized_){
         ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
         return false;
       }
       ...
       return (done);
     }
   }
   

4. 构建插件库.so：编译此功能包my_planner将会在根目录devel/lib中生成插件libmy_planner.so

5. 集成插件库到ROS：在功能包my_planner下创建my_planner_plugin.xml描述插件信息和库路径

   <library path="lib/libmy_planner">
     <class name="my_planner/MyPlanner" type="my_planner::MyPlanner" base_class_type="nav_core::BaseGlobalPlanner">
       <description>
         My planner
       description>
     class>
   library>
   

   在功能包my_planner下package.xml中导出my_planner_plugin.xml

    <depend>nav_coredepend>
    
    <export>
        <nav_core plugin="${prefix}/my_planner_plugin.xml" />
    export>
   

6. 使用插件：在turtlebots_navigation中的move_base.launch新增一行，声明使用自定义插件

   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <param name="base_global_planner" value="my_planner/MyPlanner"/>
   	...
   node>
   

7. 测试：启动仿真环境测试算法

   原算法是Dijkstra算法，规划的路径是曲线

测试算法是简单的直线规划，可以看出此时规划器已替换成自定义的规划器，实验成功

常见问题

1. /opt/ros/noetic/lib/move_base/move_base: symbol lookup error: /home/winter/ROS/ros_learning_tutorials/Lecture19/devel/lib//libmy_planner.so: undefined symbol: _ZN18base_local_planner12CostmapModelC1ERKN10costmap_2d9Costmap2DE

   解决方案：未定义符号错误undefined symbol一般是依赖配置错误导致，采用c++filt工具解析符号

   c++filt _ZN18base_local_planner12CostmapModelC1ERKN10costmap_2d9Costmap2DE
   base_local_planner::CostmapModel::CostmapModel(costmap_2d::Costmap2D const&)
   

   可以看出是base_local_planner的问题，需要在功能包CMakeLists.txt中配置base_local_planner的相关依赖。

   c++filt是什么？g++编译器有名字修饰机制,其目的是给同名的重载函数不同的、唯一的签名识别，所有函数在编译后的文件中都会生成唯一的符号，c++filt可以逆向解析符号，还原函数，定位代码。

本文的完整工程代码联系下方博主名片获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《机器人原理与技术》
• 《机器学习强基计划》
• 《计算机视觉教程》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系