45.在ROS中实现global planner(1)

前文move_base介绍(4)简单介绍move_base的全局路径规划配置,接下来我们自己实现一个全局的路径规划

1. move_base规划配置

ROS1move_base可以配置选取不同的global plannerlocal 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_plannerbase_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 bgp_loader_;
planner_ = bgp_loader_.createInstance(global_planner);

pluginlib可以参见这里

  • pluginlib::ClassLoader::createInstance根据输入参数名,加载so,并且获取到库的导出类,且创建该类的一个实例
  • planner_即为该指向该实例的指针, 有了这个对象,就可以通过该成员干活了

2.2 BaseGlobalPlanner接口

planner_定义在move_base.h#L185

boost::shared_ptr 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& plan) = 0;

      virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
                            const geometry_msgs::PoseStamped& goal, std::vector& 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(){}
  };

可以看到该类是一个接口类,需要继承该接口做相应的实现,主要接口比较简单,就两个, initializemakePlan, 顾名思义一个初始化,一个规划路径

  • 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
上面已经看到该接口定义 我们继承并对两个接口initializemakePlan实现即可

  • 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 &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


  
    
      A sample implementation of a grid based planner 
    
  

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完成后发出pathtopic

void GlobalPlanner::publishPlan(const std::vector &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 MapLocal Map中的dwa planner关闭, 只显示Full Plan

修改move_base_params.yamlplanner_frequency值, 0 只规划一次, >0 规划频率

3.7 测试结果

  • 选择空旷区域,可以看到可以正常规划,同时控制也可以启动完成,到达目的地


    image.png
  • 跨过障碍物,可以看到规划出路径,显然无法控制过去


    image.png

4. 总结

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

本文代码见sample_global_planner

你可能感兴趣的:(45.在ROS中实现global planner(1))