47.在ROS中实现global planner（3）

news2024/9/21 18:42:54

接着之前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

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

4. 优化

4.1 规划优化

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

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

简单的说在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)));
...