45.在ROS中实现global planner（1）

news2024/9/29 19:26:22

前文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);
}