45.在ROS中实现global planner（1）
前文move_base介绍（4）简单介绍move_base的全局路径规划配置，接下来我们自己实现一个全局的路径规划
1. move_base规划配置
ROS1的move_base可以配置选取不同的global planner和local planner， 默认move_base.cpp#L70中可以看到是读取该参数决定的`
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
我们可以通过配置base_global_planner和base_local_planner参数修改不同的算法
ros1 navigation中提供了3种base_global_planner, 分别是
navfn/NavfnROSglobal_planner::GlobalPlannercarrot_planner/CarrotPlanner
下面我们自己实现一个全局的路径规划，并在模拟器测试其执行效果
2. 实现原理
2.1 加载对象
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
上面我们已经知道 通过参数配置来决定加载哪一个全局规划器，继续跟踪可以看到
查看源码 move_base.cpp#L125 & move_base.h#L210
pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
planner_ = bgp_loader_.createInstance(global_planner);
pluginlib可以参见这里
-
pluginlib::ClassLoader<nav_core::BaseGlobalPlanner>::createInstance根据输入参数名，加载so，并且获取到库的导出类，且创建该类的一个实例 -
planner_即为该指向该实例的指针， 有了这个对象，就可以通过该成员干活了
2.2 BaseGlobalPlanner接口
planner_定义在move_base.h#L185
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
前面返回的planner_类型可以看到是nav_core::BaseGlobalPlanner类型，我们先来看下该类，在nav_core#L48
class BaseGlobalPlanner{
public:
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
double& cost)
{
cost = 0;
return makePlan(start, goal, plan);
}
virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual ~BaseGlobalPlanner(){}
protected:
BaseGlobalPlanner(){}
};
可以看到该类是一个接口类，需要继承该接口做相应的实现，主要接口比较简单，就两个, initialize和makePlan， 顾名思义一个初始化，一个规划路径
-
initialize
传入了name， 以及地图信息 -
makePlan
传入起点，目标点，返回plan
我们也可以看看在move_base对应接口的调用
-
move_base.cpp#L126
在创建完成立即调用完成初始化 -
move_base.cpp#L496
进行全局路径规划
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
...
}
``
在MoveBase::makeplan调用了该函数，返回的plan， 保存后用于local planner的输入
3. 实现global planner
3.1 实现步骤
实现一个自己的全局规划需要下面几个步骤
- 继承
nav_core::BaseGlobalPlanner实现接口 - 导出该实现类
- 添加
plugin.xml插件描述文件并导出 - 修改
move_base配置使用
3.2 实现接口
- 创建包
mkdir -p ~/pibot_ros/ros_ws/src
cd ~/pibot_ros/ros_ws/src
catkin_create_pkg sample_global_planner
创建完成添加一个cpp和h文件，新增一个类继承与nav_core::BaseGlobalPlanner
上面已经看到该接口定义 我们继承并对两个接口initialize和makePlan实现即可
-
initialize
初始化我们暂时先空实现
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
}
-
makePlan
规划路径的接口给我们输入起点和终点，我们输出规划出的plan(如可以规划，同时返回true，反之返回false)， 我们暂时不考虑具体实现，输出一条从起点到终点的直线路径，这应该是初中几何知识，比较简单如下
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)
{
ROS_INFO("make plan start:[%f %f], goal:[%f %f]", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
plan.clear();
float yaw = atan2(goal.pose.position.y - start.pose.position.y, goal.pose.position.x - start.pose.position.x);
int n = 0;
float goal_distance = sqrt(pow((start.pose.position.x - goal.pose.position.x), 2) + pow((start.pose.position.y - goal.pose.position.y), 2));
float delta = 0.1; // 间隔delta输出start至end的直线上的点 我们间隔0.1取直线上的所有点，放到输出的参数plan里
while (n * delta < goal_distance)
{
geometry_msgs::PoseStamped pose = goal;
pose.pose.position.x = (n * delta) * cos(yaw) + start.pose.position.x;
pose.pose.position.y = (n * delta) * sin(yaw) + start.pose.position.y;
++n;
plan.push_back(pose);
}
plan.push_back(goal); // 这里别忘了终点
return !plan.empty();
}
- 添加相应的CMakeList.txt
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}
src/planner_node.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
3.3 导出类
参考navigation里面, 添加宏导出该类
PLUGINLIB_EXPORT_CLASS(sample_global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
3.3 添加plugin.xml
添加一个bgp_plugin.xml
<library path="lib/libsample_global_planner">
<class name="sample_global_planner/GlobalPlanner" type="sample_global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>
A sample implementation of a grid based planner
</description>
</class>
</library>
3.4 编译
cd ~/pibot_ros/ros_ws
catkin_make
3.5 修改配置测试
修改~/pibot_ros/src/pibot_simulator/move_base_params.yaml
# base_global_planner: global_planner/GlobalPlanner
base_global_planner: sample_global_planner/GlobalPlanner
global_planner/GlobalPlanner ----> sample_global_planner/GlobalPlanner
- 启动模拟器
pibot_simulator
- 查看当前的
global_planner
❯ rosparam get /move_base/base_global_planner
sample_global_planner/GlobalPlanner # 输出sample_global_planner/GlobalPlanner表示插件已经被正确加载
- 启动rviz发送点位，选点导航测试
pibot_view
3.6 路径显示
上面测试可以看到可以规划已经完成， dwa的局部规划已经启动， 为了方便查看全局全规划路径的输出，我们在makeplan完成后发出path的topic
void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped> &path)
{
nav_msgs::Path gui_path;
gui_path.poses.resize(path.size());
gui_path.header.frame_id = frame_id_;
gui_path.header.stamp = ros::Time::now();
for (unsigned int i = 0; i < path.size(); i++)
{
gui_path.poses[i] = path[i];
}
plan_pub_.publish(gui_path);
}
把 rviz
Global Map和Local Map中的dwa planner关闭， 只显示Full Plan
修改
move_base_params.yaml中planner_frequency值， 0 只规划一次， >0 规划频率
3.7 测试结果
-
选择空旷区域，可以看到可以正常规划，同时控制也可以启动完成，到达目的地
image.png
-
跨过障碍物，可以看到规划出路径，显然无法控制过去
image.png
4. 总结
本文简单实现了一个global planner的插件，显然实际没啥用，不过可以作为一个模板，基于该模板实现自己的算法。后面我们将基于该模板实现可用的全局规划。
