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/NavfnROS
• global_planner::GlobalPlanner
• carrot_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 测试结果

• 选择空旷区域，可以看到可以正常规划，同时控制也可以启动完成，到达目的地
  
• 跨过障碍物，可以看到规划出路径，显然无法控制过去
  

4. 总结

本文简单实现了一个global planner的插件，显然实际没啥用，不过可以作为一个模板，基于该模板实现自己的算法。后面我们将基于该模板实现可用的全局规划。

本文代码见sample_global_planner