接着之前45.在ROS中实现global planner（1）和46.在ROS中实现global planner（2）的铺垫，在ROS中实现AStar Global Planner

1. planner package

• 照着之前的模板，修改下名称

git clone -b https://gitee.com/pibot/sample_global_planner.git astar_global_planner

再替换下所有的sample_global_planner-->astar_global_planner

• 基于这个我们新增之前的astar算法
  拷贝astar_planner.h 和astar_planner.cpp 分别至include和src目录
• 修改CMakeLists.txt
  • 添加编译

  add_library(${PROJECT_NAME}
    src/planner_node.cpp
    src/astar_planner.cpp  # 新增
  )
  

  • 添加opencv的依赖

  find_package(catkin REQUIRED
  COMPONENTS
      costmap_2d
      OpenCV)
  

  • c++11支持
    使用了c++11 cmake打开配置

  add_compile_options(-std=c++11) 
  

直接编译试下
catkin_make -DCATKIN_WHITELIST_PACKAGES=astar_global_planner
头文件报错，稍微修改下
#include "astar_global_planner/astar_planner.h"

2. 规划实现

前面加入了编译，下面我们具体新增其的使用

2.1 添加AStarPlanner

• 把之前实现好的AStarPlanner 添加为GlobalPlanner的一个成员

    std::unique_ptr<AStarPlanner> planner_; // 这里我们指定为指针
  

• 初始化接口添加对其的实例化

    planner_ = std::unique_ptr<AStarPlanner>(new AStarPlanner());
  

2.2 接口调用

AStarPlanner 主要的接口就是

bool Plan(const Mat& cost_map, const Point& start_point, const Point& end_point, std::vector<Point>& path, PlannerAction planner_action);

主要接收一个地图参数cost_map， 一个起点start_point，一个终点end_point， 输出为路径path

planner_action为回调函数 之前调试显示用，这里用不到， 传入nullptr

• 地图

地图参数在GlobalPlanner的initialize接口有指定，我们只需要在这里保存下来，保存至成员，后续函数中使用

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
  costmap_ = costmap_ros->getCostmap();

  int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
  double resolution = costmap_->getResolution();

  cv::Mat costmap_mat = cv::Mat(costmap_->getSizeInCellsY(), costmap_->getSizeInCellsX(), CV_8UC1, costmap_->getCharMap());
  costmap_mat_ = costmap_mat.clone();  // costmap_mat_保存地图的拷贝
}

• 起点&终点
  规划接口中传入了2个参数分别是起点和终点，但需要注意传入的点的坐标系为世界坐标系，并非地图的坐标，所以需要做转换，initialize传入参数costmap_ros带有转换接口worldToMap直接调用即可，如下

  unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;

  double wx = start.pose.position.x;
  double wy = start.pose.position.y;
  costmap_->worldToMap(wx, wy, start_x_i, start_y_i);

  wx = goal.pose.position.x;
  wy = goal.pose.position.y;

  costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)

• 执行规划
  只需要调用AStarPlanner的Plan接口，传入相应的参数即可

  std::vector<Point> path;
  bool vaild = planner_->Plan(costmap_mat_, {start_x_i, start_y_i}, {goal_x_i, goal_y_i}, path, nullptr);

• 输出路径
  同起点&终点相反，这里输出路径保存的是地图坐标，所有需要转换到世界坐标返回，同样使用costmap_ros的下mapToWorld接口即可
  另外AStarPlanner Plan接口输出的路径是从终点到起点的，这里需要反序列后输出

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
                             const geometry_msgs::PoseStamped& goal,
                             std::vector<geometry_msgs::PoseStamped>& plan) {
  ...

  geometry_msgs::PoseStamped pose = goal;

  // 使用path反向迭代器循环
  for (auto it = path.rbegin(); it != path.rend(); it++) {
    costmap_->mapToWorld(it->x, it->y, pose.pose.position.x, pose.pose.position.y);
    plan.push_back(pose);
  }
  
  // plan 即为需要返回和pub的路径
  ....
}

3. 测试

• 修改move_base_params.yaml中的base_global_planner

  base_global_planner: astar_global_planner/GlobalPlanner
  

• 启动模拟器
  pibot_simulator

• 查看插件是否加载

  ❯ rosparam get /move_base/base_global_planner
  astar_global_planner/GlobalPlanner
  

• 打开rviz查看
  pibot_view
  

  image.png

可以看出来AStar Global Planner已经生效了，规划路径也出来了， 很不幸的是local planner一直无法正常运行，显然这个路径规划的太贴近障碍物。导致规划的结果无法在实际机器人场景使用， 后续我们看看如何做优化。

4. 优化

4.1 规划优化

我们的A* 算法没有考虑到障碍无的距离，导致可能紧挨着障碍物，我们需要在启发函数新增里障碍物的距离值，以使得规划路径尽量远离障碍物。

在ROS中的地图是cost map，所谓cost map也就是每个grid点有自己的cost， 如官方的这张图片

image.png

简单的说在ROS中，未知区域值255， 障碍物为254，其他就是远离障碍物就越越小

• 这样我们在计算H的函数新增个参数，同时添加相应的weight

  float Point2PointPlanner::CalcH(const Point& current_point, const Point& end_point) {
    ...
    return min_diff * kCornerCost + (max_diff - min_diff) * kLineCost;
  }
  

  改为

  float Point2PointPlanner::CalcH(const Point& current_point, const Point& end_point, float cost) {
    ...
    return min_diff * kCornerCost + (max_diff - min_diff) * kLineCost + cost * kCost;
    }
  

• 计算时传入每个grid的cost

  ...
    open_list_.emplace(start_point, 0, CalcH(start_point, end_point, cost_map_.at<unsigned char>(start_point.y, start_point.x)));
  
  ...`
  
    all_grid_[index].Update(neighbor, point, G, CalcH(neighbor, end_point, cost_map_.at<unsigned char>(neighbor.y, neighbor.x)));
  ...
  

再次编译测试，结果如下图，可以看到规划的路径在道路中间，并且local planner可以正常工作。`

image.png

4.2 路径优化

可以看到，规划的路径是还存在瑕疵，路径是锯齿状的，对local planner的控制有一定影响，我们可以添加平滑处理，使得路径更为平滑
参考ROS:一种简单的基于global_planner的全局路径优化方法得到效果如下

image.png