通俗易懂的Astar算法

(1)main.cpp

/**********************************************************************
* Copyright (C) 2015-2020
*
* NodeName: planning
* FileName: main.cpp
*
* Description: 
    
*
* History:
* guohui zhang         2020/07/23    1.0.0    build this module.
************************************************************************/
#include "ros/ros.h"
#include "ros/time.h"
#include "ros/duration.h"
#include "astar.h"

void printMap(char map[MAX_X][MAX_Y],int width,int height) {
  for (int i = 0; i<width; i++) {
    for (int j = 0; j<height; j++) {
      printf("%c\t",map[i][j]);
    }
    printf("\n");
  }
}

int main(int argc, const char * argv[]) {
  cout<<"---map---"<<endl;
  //*******初始化地图矩阵 0代表障碍*******
  char map_data[MAX_X][MAX_Y] = {
    {'1','0','0','1','0','1','1','1','1','1','1','0','0','1','0','1','1','1','1','1',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'0','0','0','1','0','1','1','1','1','1','0','0','0','1','0','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','0','1','0','0','1','0','1','1','1','1','0',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','1','1','0','0','1','0','1','1','1','1','1',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'0','0','0','1','0','1','1','1','1','1','0','0','0','1','0','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','0','1','0','0','1','0','1','1','1','1','0',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','1','1','0','0','1','0','1','1','1','1','1',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'0','0','0','1','0','1','1','1','1','1','0','0','0','1','0','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','0','1','0','0','1','0','1','1','1','1','0',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','1','1','0','0','1','0','1','1','1','1','1',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'0','0','0','1','0','1','1','1','1','1','0','0','0','1','0','1','1','1','1','1',},
    {'1','0','0','1','0','1','1','1','1','0','1','0','0','1','0','1','1','1','1','0',},
    {'1','1','1','1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1','1',},
    {'1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1','1',},
    {'1','1','0','0','1','1','1','1','1','1','1','1','0','0','1','1','1','1','1','1',},
    {'1','0','1','1','1','1','1','1','1','1','1','0','1','1','1','1','1','1','1','1',},
  };

  //*******建立grid_map*******
  std::vector<std::vector<astar_node>> grid_map;
  for (int i = 0; i < MAX_X; ++i) {
    vector<astar_node> temp_node;
    for (int j = 0; j < MAX_Y; ++j) {
      astar_node new_node;
      new_node.x = i;
      new_node.y = j;
      if (map_data[i][j] == '0') {
        new_node.type = TYPE::BLOCK;
      }
      temp_node.push_back(new_node);
    }
    grid_map.push_back(temp_node);
  }
  //*******实例化astar并调用*******
  Astar astar;
  astar_node start_point = grid_map.at(0).at(0);
  astar_node end_point = grid_map.at(39).at(19);
  std::vector<astar_node> result;
  bool path_flag = astar.FindAstarPath(start_point, end_point, grid_map, result);
  if (false == path_flag) {
    cout<<"---没有找到路径---"<<endl;
    return 0;
  }
  cout<<"---下面是路径点---"<<endl;
  for (auto single_node : result) {
    map_data[single_node.x][single_node.y] = '*';
    cout << "x|y: " << single_node.x << " | " << single_node.y << endl;
  }
  cout<<"---打印路径---"<<endl;
  printMap(map_data, MAX_X, MAX_Y);
  return 0;
}

(2)astar.cpp

/**********************************************************************
* Copyright (C) 2015-2020
*
* NodeName: astar
* FileName: astar.cpp
*
* Description: relax astar algorithm
    
*
* History:
* guohui zhang         2020/07/23    1.0.0    build this module.
************************************************************************/
#include "astar.h"

bool SortRule(const astar_node *temp_node1, const astar_node *temp_node2) {
  return temp_node1->f_cost < temp_node2->f_cost;
}

Astar::Astar() {
  open_list_.clear();
  close_list_.clear();
}
Astar::~Astar() {
  open_list_.clear();
  close_list_.clear();
}

int Astar::UpdateFValue(const astar_node *temp_node) const{
  return temp_node->g_cost + temp_node->h_cost;
}

int Astar::ComputeGValue(const astar_node &temp_node1, const astar_node &temp_node2) const{
  return temp_node1.g_cost + temp_node2.g_cost;
}

int Astar::ComputeHValue(const astar_node *temp_node1, const astar_node &temp_node2) const{
  //曼哈顿距离
  return (abs(temp_node1->x - temp_node2.x) + abs(temp_node1->y - temp_node2.y)) * 10;
  //欧式距离
  // return hypot(temp_node1->x - temp_node2.x, temp_node1->y - temp_node2.y) * 10;

}

bool Astar::IsAngleNode(const astar_node &temp_node1, const astar_node *temp_node2) const{
  if (fabs(temp_node1.x - temp_node2->x) > 0 && fabs(temp_node1.y - temp_node2->y) > 0) return true;
  return false;
}

bool Astar::IsInCloseList(const astar_node temp_node) const{
  for (int i = 0; i < close_list_.size(); ++i) {
    if (*close_list_.at(i) == temp_node) {
      return true;
    }
  }
  return false;
}

bool Astar::IsInOpenList(const astar_node temp_node, int &index) const{
  for (int i = 0; i < open_list_.size(); ++i) {
    if (*open_list_.at(i) == temp_node) {
      index = i;
      return true;
    }
  }
  return false;
}

bool Astar::FindAstarPath(const astar_node &start_point, const astar_node &end_point, 
  const std::vector<std::vector<astar_node>> &grid_map, std::vector<astar_node> &result) {
  if (!grid_map.size()) return false;
  if (end_point.type == TYPE::BLOCK) return false;
  if (start_point == end_point) {
    result.emplace_back(end_point);
    return true;
  }
  open_list_.emplace_back(new astar_node(start_point));
  astar_node *current_ptr = NULL;

  while (open_list_.size()) {
    current_ptr = open_list_.front();
    open_list_.erase(open_list_.begin());
    close_list_.emplace_back(current_ptr);
    //*******判断结束条件*******
    if (*current_ptr == end_point) {
      // cout<<"have find way"<<endl;
      break;
    }
    std::vector<astar_node> neighbour;
    //*******找到当前节点的相邻点*******
    GetNeighboringNode(grid_map, *current_ptr, neighbour);
    for (const auto &check_node : neighbour) {
      //*******检测是否在Close表*******
      if (IsInCloseList(check_node)) continue;
      int temp_g_cost = 0; //假设经过当前点到达该点的g值
      if (IsAngleNode(check_node, current_ptr)) { //判断是否是当前点四角的格点
        temp_g_cost = current_ptr->g_cost + 14;
      } else {
        temp_g_cost = current_ptr->g_cost + 10;
      }
      // *******检测是否在Open表*******
      int index = -1;
      if (IsInOpenList(check_node, index)) { //在Open表
        if (temp_g_cost < open_list_.at(index)->g_cost) { //根据g值判断经过当前点到达是否更近
          open_list_.at(index)->g_cost = temp_g_cost;
          open_list_.at(index)->f_cost = UpdateFValue(open_list_.at(index));
          open_list_.at(index)->parent = current_ptr;
        }
      } else { //不在Open表
        astar_node *new_node = new astar_node(check_node);
        new_node->g_cost = temp_g_cost;
        new_node->h_cost = ComputeHValue(new_node, end_point);
        new_node->f_cost = UpdateFValue(new_node);
        new_node->parent = current_ptr;
        open_list_.emplace_back(new_node);
      }
      std::sort(open_list_.begin(), open_list_.end(), SortRule); //按照f值排序open表
    }
  }
  if (*current_ptr != end_point) return false;
  result.clear();
  while(close_list_.size() && current_ptr != NULL) {
    result.emplace_back(*current_ptr);
    current_ptr = current_ptr->parent;
  }
  std::reverse(result.begin(),result.end());
  // std::cout << "result.size(): " << result.size() << std::endl;
  return true;
}


void Astar::GetNeighboringNode(const std::vector<std::vector<astar_node>> &map, 
  const astar_node &temp_node, std::vector<astar_node> &neighbour) const{
  neighbour.clear();
  if (temp_node.x < MAX_X-1) { //右 
    if (map[temp_node.x+1][temp_node.y].type != TYPE::BLOCK) { //滤出不可通行点
      neighbour.push_back(map[temp_node.x+1][temp_node.y]);
    }
  }
  if (temp_node.x > 0) { //左
    if (map[temp_node.x-1][temp_node.y].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x-1][temp_node.y]);
    }
  }
  if (temp_node.y < MAX_Y-1) { //下
    if (map[temp_node.x][temp_node.y+1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x][temp_node.y+1]);
    }
  }
  if (temp_node.y > 0) { //上
    if (map[temp_node.x][temp_node.y-1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x][temp_node.y-1]);
    }
  }
  if (temp_node.x < MAX_X-1 && temp_node.y < MAX_Y-1) { //右下
    if (map[temp_node.x+1][temp_node.y+1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x+1][temp_node.y+1]);
    }
  }
  if (temp_node.x > 0 && temp_node.y < MAX_Y-1) { //左下
    if (map[temp_node.x-1][temp_node.y+1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x-1][temp_node.y+1]);
    }
  }
  if (temp_node.x < MAX_X-1 && temp_node.y > 0) { //右上
    if (map[temp_node.x+1][temp_node.y-1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x+1][temp_node.y-1]);
    }
  }
  if (temp_node.x > 0 && temp_node.y > 0) { //左上
    if (map[temp_node.x-1][temp_node.y-1].type != TYPE::BLOCK) {
      neighbour.push_back(map[temp_node.x-1][temp_node.y-1]);
    }
  }
}

(3)astar.h

#ifndef SRC_ASTAR_H_
#define SRC_ASTAR_H_

#include <stdio.h>
#include <vector>
#include <iostream>
#include <algorithm>
using namespace std;

//地图最大值
#define MAX_X 40
#define MAX_Y 20

enum class TYPE {
  UNKNOWN,
  BLOCK   //不可通行
};

struct astar_node {
  int x;
  int y;
  TYPE type;
  int f_cost;
  int g_cost;
  int h_cost;
  astar_node *parent;
  astar_node() {
    reset();
  }
  astar_node(const astar_node &temp) {
    x = temp.x;
    y = temp.y;
    type = temp.type;
    f_cost = temp.f_cost;
    g_cost = temp.g_cost;
    h_cost = temp.h_cost;
    parent = temp.parent;
  }
  void reset() {
    x = -1;
    y = -1;
    type = TYPE::UNKNOWN;
    f_cost = std::numeric_limits<int>::max();
    g_cost = std::numeric_limits<int>::max();
    h_cost = std::numeric_limits<int>::max();
    parent = NULL;
  }
  bool operator == (const astar_node &temp) const{
    if (x == temp.x && y == temp.y) {
      return true;
    }
    return false;
  }
  bool operator != (const astar_node &temp) const{
    if (x != temp.x || y != temp.y) {
      return true;
    }
    return false;
  }
};

class Astar {
private:
  std::vector<astar_node *> open_list_;
  std::vector<astar_node *> close_list_;
public:
  Astar();
  ~Astar();
  bool FindAstarPath(const astar_node &start_point, const astar_node &end_point, 
                     const std::vector<std::vector<astar_node>> &map, std::vector<astar_node> &result);
private:
  int UpdateFValue(const astar_node *temp_node) const;
  int ComputeGValue(const astar_node &temp_node1, const astar_node &temp_node2) const;
  int ComputeHValue(const astar_node *temp_node1, const astar_node &temp_node2) const;
  bool IsAngleNode(const astar_node &temp_node1, const astar_node *temp_node2) const;
  bool IsInCloseList(const astar_node temp_node) const;
  bool IsInOpenList(const astar_node temp_node, int &index) const;
  void GetNeighboringNode(const std::vector<std::vector<astar_node>> &map, 
                          const astar_node &temp_node, std::vector<astar_node> &neighbour) const;
};

#endif // SRC_ASTAR_H_