ROS从入门到精通5-4:路径规划插件开发案例(以A*算法为例)
Posted Mr.Winter`
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了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
用于生成自定义全局路径规划插件
-
构造基类:由于全局路径规划插件全部继承于
nav_core
功能包的BaseGlobalPlanner
类,因此无需构造 -
构造插件类:在
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); ; ;
-
注册插件:在
my_planner/src
中新建my_planner.cpp
使用PLUGINLIB_EXPORT_CLASS
宏注册插件,限于篇幅不列出完整代码,该代码完全复制官方carrot_planner
作为测试用例。#include <angles/angles.h> #include <my_planner/my_planner.h> #include <pluginlib/class_list_macros.h> #include <tf2/convert.h> #include <tf2/utils.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> //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);
-
构建插件库
.so
:编译此功能包my_planner
将会在根目录devel/lib
中生成插件libmy_planner.so
-
集成插件库到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_core</depend><!-- 注意此依赖,否则找不到自定义规划器 --> <!-- The export tag contains other, unspecified, tags --> <export> <nav_core plugin="$prefix/my_planner_plugin.xml" /> </export>
-
使用插件:在
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>
-
测试:启动仿真环境测试算法
原算法是Dijkstra算法,规划的路径是曲线
测试算法是简单的直线规划,可以看出此时规划器已替换成自定义的规划器,实验成功
常见问题
-
/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从入门到精通5-4:路径规划插件开发案例(以A*算法为例)的主要内容,如果未能解决你的问题,请参考以下文章