个人学习记录,参考的博客文末都有链接
文章目录
创建功能包 fixed_route
1. 自定义路径规划头文件 my_global_planner.h
/** include the libraries you need in your planner here */
/** for global path planner interface */
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <tf/transform_broadcaster.h>
using std::string;
#ifndef GLOBAL_PLANNER_CPP
#define GLOBAL_PLANNER_CPP
namespace my_global_planner {
class MyGlobalPlanner : public nav_core::BaseGlobalPlanner {
public:
MyGlobalPlanner();
MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/** overridden classes from interface nav_core::BaseGlobalPlanner **/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan
);
};
};
#endif
- 需要包含路径规划器所需的核心 ROS 库。路径规划器需要使用<costmap_2d/costmap_2d_ros.h> 和 <costmap_2d/costmap_2d.h>, 而costmap_2d::Costmap2D 类作为输入的地图类。当定义为插件时,路径规划器类将自动访问此地图。没有必要订阅costmap2d来获取ROS的代价地图。
- <nav_core/base_global_planner.h> 用于导入接口 nav_core::BaseGlobalPlanner,这是插件必须要继承的父类。
2. 自定义路径规划算法 my_global_planner.cpp
#include <pluginlib/class_list_macros.h>
#include "fixed_route/my_global_planner.h"
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(my_global_planner::MyGlobalPlanner, nav_core::BaseGlobalPlanner)
using namespace std;
//Default Constructor
namespace my_global_planner {
MyGlobalPlanner::MyGlobalPlanner (){
ROS_ERROR("hello MyGlobalPlanner");
}
MyGlobalPlanner::MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
initialize(name, costmap_ros);
ROS_ERROR("hello MyGlobalPlanner initialize");
}
void MyGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
ROS_ERROR("hello MyGlobalPlanner initialize");
}
bool MyGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan ){
ROS_ERROR("hello MyGlobalPlanner makePlan");
plan.push_back(start);
for (int i=0; i<20; i++){
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = -2.5+(0.05*i);
new_goal.pose.position.y = -3.5+(0.05*i);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
plan.push_back(goal);
return true;
}
};
在cpp文件中需要完善 initialize 和 makePlan 函数
- 构造函数 MyGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 用于初始化代价地图,即将用于规划的代价地图(costmap_ros)以及规划器的名称(name)。
- 默认构造函数 MyGlobalPlanner() 即使用默认值作为初始化。方法 initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 是 BaseGlobalPlanner 的初始化函数,用于初始化 costmap,参数为用于规划的代价地图(costmap_ros)和规划器的名称(name)。
- bool makePlan(start,goal,plan)的方法必须要重写。最终规划将存储在方法的参数 std::vector<geometry_msgs::PoseStamped>& plan 中。
- 注册规划器作为 BaseGlobalPlanner 插件:这是通过指令 PLUGINLIB_EXPORT_CLASS(my_global_planner::MyGlobalPlanner, nav_core::BaseGlobalPlanner) 完成的。为此,必须包含库 #include
<pluginlib/class_list_macros.h - makePlan() 方法实现:在开始和目标参数用来获取初始位置和目标位置。在该说明性实现中,以起始位置(plan.push_back(start)))作为开始的规划变量。然后将目标位置作为最后一个位置插入规划。然后,此规划路径将发送到 move_base 全局规划模块,该模块将通过 ROS 话题 nav_msgs/Path 进行发布,然后将由本地规划模块接收。
3. 配置 CMakeLists.txt 文件
要编译上面创建的全局规划库,必须将其添加到 CMakeLists.txt 中。添加代码:
add_library(my_global_planner_lib src/my_global_planner.cpp)
4. 配置 package.xml 文件
注册插件到ROS包系统
为了让 pluginlib 在所有 ROS 程序包上查询系统上的所有可用插件,每个程序包都必须明确指定其导出的插件,哪些程序包库包含这些插件。插件提供者必须在其导出标记块中的package.xml 中指向其插件描述文件。
在 package.xml 的文件中的 export 标签里加入下面这句
<nav_core plugin="${prefix}/my_global_planner_plugin.xml" />
注意:为了使上述导出命令正常工作,提供程序包必须直接依赖于包含插件接口的程序包,这在全局规划程序的情况下是 nav_core。因此,其 package.xml 中必须包含以下内容:
<build_depend>nav_core</build_depend>
<run_depend>nav_core</run_depend>
完整:
<build_depend>nav_core</build_depend>
<exec_depend>nav_core</exec_depend>
<export>
<!-- Other tools can request additional information be placed here -->
<nav_core plugin="${prefix}/my_global_planner_plugin.xml" />
</export>
回到工作目录 catkin_make 进行编译,完成后可以在 devel/lib 下有一个 libmy_global_planner_lib.so。
继续编写插件描述文件
5. 自定义插件描述文件 my_global_planner_plugin.xml
现在为全局规划类创建插件,将其集成到 move_base 包的全局规划模块nav_core::BaseGlobalPlanner 中。
<!-- 注意修改lib的路径及名称 -->
<library path="lib/libmy_global_planner_lib">
<!-- name是命名空间名/类名,move_base_param.yaml全局规划名称一致,type是命名空间名::类名 -->
<class name="my_global_planner/MyGlobalPlanner" type="my_global_planner::MyGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>This is a global planner plugin by zhank fixed_route project.</description>
</class>
</library>
my_global_planner_plugin.xml 路径与 CMakeLists.txt 文件和 package.xml 文件处于同级目录。
编译运行后,使用如下命令可以查看注册的插件
rospack plugins --attrib=plugin nav_core
6. move_base 中使用插件
方法一
(1) 打开自己的 move_base.launch 配置文件,在 move_base 启动节点后添加
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_global_planner" value="my_global_planner/MyGlobalPlanner"/>
- 需要注意的是,规划器的名称是 my_global_planner/MyGlobalPlanner,跟指定在my_global_planner_plugin.xml 文件名称一致。
(2) global_planner_params.yaml 调用插件的配置参数
<rosparam file="$(find xxx)/config/global_planner_params.yaml" command="load" />
MyGlobalPlanner:
allow_unknown: true
default_tolerance: 0.0
visualize_potential: false
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false
完整:
<?xml version="1.0"?>
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<!-- TebPlanner -->
<param name="base_local_planner" value="teb_planner/TebPlanner" />
<rosparam file="$(find xxx)/config/teb_planner_params.yaml" command="load" />
<!-- GlobalPlanner -->
<param name="base_global_planner" value="my_global_planner/MyGlobalPlanner" />
<rosparam file="$(find xxx)/config/global_planner_params.yaml" command="load" />
<rosparam file="$(find xxx)/config/teb/move_base_params.yaml" command="load" />
<rosparam file="$(find xxx)/config/teb/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find xxx)/config/teb/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find xxx)/config/teb/local_costmap_params.yaml" command="load" />
<rosparam file="$(find xxx)/config/teb/global_costmap_params.yaml" command="load" />
</node>
</launch>
方法二
(1) move_base_params.yaml 使用插件
# 命名空间名/类名 与my_global_planner_plugin.xml中的name一致
base_global_planner: "my_global_planner/MyGlobalPlanner"
(2) global_planner_params.yaml 调用插件的配置参数
ns: MyGlobalPlanner
allow_unknown: true
default_tolerance: 0.0
visualize_potential: false
use_dijkstra: true
use_quadratic: true
use_grid_path: false
old_navfn_behavior: false
(3) gmapping_navigation.launch 在 launch 文件里找到 ns 为 MyGlobalPlanner 的配置参数
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find xxx)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find xxx)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find xxx)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find xxx)/param/global_planner_params.yaml" command="load" ns="MyGlobalPlanner"/>
<rosparam file="$(find xxx)/param/local_planner_params.yaml" command="load" ns="DWAPlannerROS"/>
</node>
参考博客: