ROS实现A*算法

简介

• A*算法是一种基于启发式搜索的全局寻路算法，它的工作原理是每次从待处理的节点中挑选出一个“最有望”的节点先处理，这个所谓“最有望”的判断来源于一个评价函数 f(n)=g(n)+h(n)。g(n) 表示从起点到当前定点的实际距离，h(n) 是当前节点到目标节点的启发式估计，也称为启发式函数或者启发式估价。这个启发式函数是问题相关的，通常由问题场景和专业知识来决定。
• A*算法的效率和性能很大程度上取决于启发式函数h(n)的选择。

一、ROS/C++代码

开源项目仓库

1、栅格地图头文件OccMapTransform.h：

//
// Created by lihao on 19-7-9.
//
#ifndef OCCMAPTRANSFORM_H
#define OCCMAPTRANSFORM_H

#include <iostream>
#include <cmath>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/tf.h>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

//-------------------------------- Class---------------------------------//
class OccupancyGridParam{

public: // Interface
    void GetOccupancyGridParam(nav_msgs::OccupancyGrid OccGrid);
    void Image2MapTransform(Point& src_point, Point2d& dst_point);
    void Map2ImageTransform(Point2d& src_point, Point& dst_point);

private: // Private function

public:  // Public variable
    double resolution;
    int height;
    int width;
    // Origin pose
    double x;
    double y;
    double theta;

private: // Private variable
    // Transform
    Mat R;
    Mat t;
};
#endif //OCCMAPTRANSFORM_H

2、栅格地图源文件OccMapTransform.cpp：

//
// Created by lihao on 19-7-10.
//

#include "OccMapTransform.h"


void OccupancyGridParam::GetOccupancyGridParam(nav_msgs::OccupancyGrid OccGrid)
{
    // Get parameter
    resolution = OccGrid.info.resolution;
    height = OccGrid.info.height;
    width = OccGrid.info.width;
    x = OccGrid.info.origin.position.x;
    y = OccGrid.info.origin.position.y;

    double roll, pitch, yaw;
    geometry_msgs::Quaternion q = OccGrid.info.origin.orientation;
    tf::Quaternion quat(q.x, q.y, q.z, q.w); // x, y, z, w
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    theta = yaw;

    // Calculate R, t
    R = Mat::zeros(2,2, CV_64FC1);
    R.at<double>(0, 0) = resolution * cos(theta);
    R.at<double>(0, 1) = resolution * sin(-theta);
    R.at<double>(1, 0) = resolution * sin(theta);
    R.at<double>(1, 1) = resolution * cos(theta);
    t = Mat(Vec2d(x, y), CV_64FC1);
}

void OccupancyGridParam::Image2MapTransform(Point& src_point, Point2d& dst_point)
{
    // Upside down
    Mat P_src = Mat(Vec2d(src_point.x, height - 1 - src_point.y), CV_64FC1);
    // Rotate and translate
    Mat P_dst = R * P_src + t;

    dst_point.x = P_dst.at<double>(0, 0);
    dst_point.y = P_dst.at<double>(1, 0);
}

void OccupancyGridParam::Map2ImageTransform(Point2d& src_point, Point& dst_point)
{
    Mat P_src = Mat(Vec2d(src_point.x, src_point.y), CV_64FC1);
    // Rotate and translate
    Mat P_dst = R.inv() * (P_src - t);
    // Upside down
    dst_point.x = round(P_dst.at<double>(0, 0));
    dst_point.y = height - 1 - round(P_dst.at<double>(1, 0));
}

3、Astar.h:

//
// Created by lihao on 19-7-9.
//

#ifndef ASTAR_H
#define ASTAR_H

#include <iostream>
#include <queue>
#include <unordered_map>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;


namespace pathplanning{

enum NodeType{
    obstacle = 0,
    free,
    inOpenList,
    inCloseList
};

struct Node{
    Point point;  // node coordinate
    int F, G, H;  // cost
    Node* parent; // parent node

    Node(Point _point = Point(0, 0)):point(_point), F(0), G(0), H(0), parent(NULL)
    {
    }
};

struct cmp
{
    bool operator() (pair<int, Point> a, pair<int, Point> b) // Comparison function for priority queue
    {
        return a.first > b.first; // min heap
    }
};


struct AstarConfig{
    bool Euclidean;         // true/false
    int OccupyThresh;       // 0~255
    int InflateRadius;      // integer

    AstarConfig(bool _Euclidean = true, int _OccupyThresh = -1, int _InflateRadius = -1):
        Euclidean(_Euclidean), OccupyThresh(_OccupyThresh), InflateRadius(_InflateRadius)
    {
    }
};

class Astar{

public:
    // Interface function
    void InitAstar(Mat& _Map, AstarConfig _config = AstarConfig());
    void InitAstar(Mat& _Map, Mat& Mask, AstarConfig _config = AstarConfig());
    void PathPlanning(Point _startPoint, Point _targetPoint, vector<Point>& path);
    void DrawPath(Mat& _Map, vector<Point>& path, InputArray Mask = noArray(), Scalar color = Scalar(0, 0, 255),
            int thickness = 1, Scalar maskcolor = Scalar(255, 255, 255));

    inline int point2index(Point point) {
        return point.y * Map.cols + point.x;
    }
    inline Point index2point(int index) {
        return Point(int(index / Map.cols), index % Map.cols);
    }

private:
    void MapProcess(Mat& Mask);
    Node* FindPath();
    void GetPath(Node* TailNode, vector<Point>& path);

private:
    //Object
    Mat Map;
    Point startPoint, targetPoint;
    Mat neighbor;

    Mat LabelMap;
    AstarConfig config;

    priority_queue<pair<int, Point>, vector<pair<int, Point>>, cmp> OpenList; // open list
    unordered_map<int, Node*> OpenDict; // open dict
    vector<Node*> PathList;  // path list
};

}
#endif //ASTAR_H

4、Astar.cpp:

//
// Created by lihao on 19-7-9.
//

#include "Astar.h"

namespace pathplanning{

void Astar::InitAstar(Mat& _Map, AstarConfig _config)
{
    Mat Mask;
    InitAstar(_Map, Mask, _config);
}

void Astar::InitAstar(Mat& _Map, Mat& Mask, AstarConfig _config)
{
    char neighbor8[8][2] = {
            {-1, -1}, {-1, 0}, {-1, 1},
            {0, -1},            {0, 1},
            {1, -1},   {1, 0},  {1, 1}
    };

    Map = _Map;
    config = _config;
    neighbor = Mat(8, 2, CV_8S, neighbor8).clone();

    MapProcess(Mask);
}

void Astar::PathPlanning(Point _startPoint, Point _targetPoint, vector<Point>& path)
{
    // Get variables
    startPoint = _startPoint;
    targetPoint = _targetPoint;

    // Path Planning
    Node* TailNode = FindPath();
    GetPath(TailNode, path);
}

void Astar::DrawPath(Mat& _Map, vector<Point>& path, InputArray Mask, Scalar color,
        int thickness, Scalar maskcolor)
{
    if(path.empty())
    {
        cout << "Path is empty!" << endl;
        return;
    }
    _Map.setTo(maskcolor, Mask);
    for(auto it:path)
    {
        rectangle(_Map, it, it, color, thickness);
    }
}

void Astar::MapProcess(Mat& Mask)
{
    int width = Map.cols;
    int height = Map.rows;
    Mat _Map = Map.clone();

    // Transform RGB to gray image
    if(_Map.channels() == 3)
    {
        cvtColor(_Map.clone(), _Map, cv::COLOR_BGR2GRAY);
    }

    // Binarize
    if(config.OccupyThresh < 0)
    {
        threshold(_Map.clone(), _Map, 0, 255, cv::THRESH_OTSU);
    } else
    {
        threshold(_Map.clone(), _Map, config.OccupyThresh, 255, cv::THRESH_BINARY);
    }

    // Inflate
    Mat src = _Map.clone();
    if(config.InflateRadius > 0)
    {
        Mat se = getStructuringElement(MORPH_ELLIPSE, Size(2 * config.InflateRadius, 2 * config.InflateRadius));
        erode(src, _Map, se);
    }

    // Get mask
    bitwise_xor(src, _Map, Mask);

    // Initial LabelMap
    LabelMap = Mat::zeros(height, width, CV_8UC1);
    for(int y=0;y<height;y++)
    {
        for(int x=0;x<width;x++)
        {
            if(_Map.at<uchar>(y, x) == 0)
            {
                LabelMap.at<uchar>(y, x) = obstacle;
            }
            else
            {
                LabelMap.at<uchar>(y, x) = free;
            }
        }
    }
}

Node* Astar::FindPath()
{
    int width = Map.cols;
    int height = Map.rows;
    Mat _LabelMap = LabelMap.clone();

    // Add startPoint to OpenList
    Node* startPointNode = new Node(startPoint);
    OpenList.push(pair<int, Point>(startPointNode->F, startPointNode->point));
    int index = point2index(startPointNode->point);
    OpenDict[index] = startPointNode;
    _LabelMap.at<uchar>(startPoint.y, startPoint.x) = inOpenList;

    while(!OpenList.empty())
    {
        // Find the node with least F value
        Point CurPoint = OpenList.top().second;
        OpenList.pop();
        int index = point2index(CurPoint);
        Node* CurNode = OpenDict[index];
        OpenDict.erase(index);

        int curX = CurPoint.x;
        int curY = CurPoint.y;
        _LabelMap.at<uchar>(curY, curX) = inCloseList;

        // Determine whether arrive the target point
        if(curX == targetPoint.x && curY == targetPoint.y)
        {
            return CurNode; // Find a valid path
        }

        // Traversal the neighborhood
        for(int k = 0;k < neighbor.rows;k++)
        {
            int y = curY + neighbor.at<char>(k, 0);
            int x = curX + neighbor.at<char>(k, 1);
            if(x < 0 || x >= width || y < 0 || y >= height)
            {
                continue;
            }
            if(_LabelMap.at<uchar>(y, x) == free || _LabelMap.at<uchar>(y, x) == inOpenList)
            {
                // Determine whether a diagonal line can pass
                int dist1 = abs(neighbor.at<char>(k, 0)) + abs(neighbor.at<char>(k, 1));
                if(dist1 == 2 && _LabelMap.at<uchar>(y, curX) == obstacle && _LabelMap.at<uchar>(curY, x) == obstacle)
                    continue;

                // Calculate G, H, F value
                int addG, G, H, F;
                if(dist1 == 2)
                {
                    addG = 14;
                }
                else
                {
                    addG = 10;
                }
                G = CurNode->G + addG;
                if(config.Euclidean)
                {
                    int dist2 = (x - targetPoint.x) * (x - targetPoint.x) + (y - targetPoint.y) * (y - targetPoint.y);
                    H = round(10 * sqrt(dist2));
                }
                else
                {
                    H = 10 * (abs(x - targetPoint.x) + abs(y - targetPoint.y));
                }
                F = G + H;

                // Update the G, H, F value of node
                if(_LabelMap.at<uchar>(y, x) == free)
                {
                    Node* node = new Node();
                    node->point = Point(x, y);
                    node->parent = CurNode;
                    node->G = G;
                    node->H = H;
                    node->F = F;
                    OpenList.push(pair<int, Point>(node->F, node->point));
                    int index = point2index(node->point);
                    OpenDict[index] = node;
                    _LabelMap.at<uchar>(y, x) = inOpenList;
                }
                else // _LabelMap.at<uchar>(y, x) == inOpenList
                {
                    // Find the node
                    int index = point2index(Point(x, y));
                    Node* node = OpenDict[index];
                    if(G < node->G)
                    {
                        node->G = G;
                        node->F = F;
                        node->parent = CurNode;
                    }
                }
            }
        }
    }

    return NULL; // Can not find a valid path
}

void Astar::GetPath(Node* TailNode, vector<Point>& path)
{
    PathList.clear();
    path.clear();

    // Save path to PathList
    Node* CurNode = TailNode;
    while(CurNode != NULL)
    {
        PathList.push_back(CurNode);
        CurNode = CurNode->parent;
    }

    // Save path to vector<Point>
    int length = PathList.size();
    for(int i = 0;i < length;i++)
    {
        path.push_back(PathList.back()->point);
        PathList.pop_back();
    }

    // Release memory
    while(OpenList.size()) {
        Point CurPoint = OpenList.top().second;
        OpenList.pop();
        int index = point2index(CurPoint);
        Node* CurNode = OpenDict[index];
        delete CurNode;
    }
    OpenDict.clear();
}

}

5、主函数main.cpp:

#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <opencv2/opencv.hpp>
#include "Astar.h"
#include "OccMapTransform.h"

using namespace cv;
using namespace std;

//-------------------------------- Global variables ---------------------------------//
// Subscriber
ros::Subscriber map_sub;
ros::Subscriber startPoint_sub;
ros::Subscriber targetPoint_sub;
// Publisher
ros::Publisher mask_pub;
ros::Publisher path_pub;

// Object
nav_msgs::OccupancyGrid OccGridMask;
nav_msgs::Path path;
pathplanning::AstarConfig config;
pathplanning::Astar astar;
OccupancyGridParam OccGridParam;
Point startPoint, targetPoint;

// Parameter
double InflateRadius;
bool map_flag;
bool startpoint_flag;
bool targetpoint_flag;
bool start_flag;
int rate;

//-------------------------------- Callback function ---------------------------------//
void MapCallback(const nav_msgs::OccupancyGrid& msg)
{
    // Get parameter
    OccGridParam.GetOccupancyGridParam(msg);

    // Get map
    int height = OccGridParam.height;
    int width = OccGridParam.width;
    int OccProb;
    Mat Map(height, width, CV_8UC1);
    for(int i=0;i<height;i++)
    {
        for(int j=0;j<width;j++)
        {
            OccProb = msg.data[i * width + j];
            OccProb = (OccProb < 0) ? 100 : OccProb; // set Unknown to 0
            // The origin of the OccGrid is on the bottom left corner of the map
            Map.at<uchar>(height-i-1, j) = 255 - round(OccProb * 255.0 / 100.0);
        }
    }

    // Initial Astar
    Mat Mask;
    config.InflateRadius = round(InflateRadius / OccGridParam.resolution);
    astar.InitAstar(Map, Mask, config);

    // Publish Mask
    OccGridMask.header.stamp = ros::Time::now();
    OccGridMask.header.frame_id = "map";
    OccGridMask.info = msg.info;
    OccGridMask.data.clear();
    for(int i=0;i<height;i++)
    {
        for(int j=0;j<width;j++)
        {
            OccProb = Mask.at<uchar>(height-i-1, j) * 255;
            OccGridMask.data.push_back(OccProb);
        }
    }

    // Set flag
    map_flag = true;
    startpoint_flag = false;
    targetpoint_flag = false;
}

void StartPointCallback(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
    Point2d src_point = Point2d(msg.pose.pose.position.x, msg.pose.pose.position.y);
    OccGridParam.Map2ImageTransform(src_point, startPoint);

    // Set flag
    startpoint_flag = true;
    if(map_flag && startpoint_flag && targetpoint_flag)
    {
        start_flag = true;
    }

//    ROS_INFO("startPoint: %f %f %d %d", msg.pose.pose.position.x, msg.pose.pose.position.y,
//             startPoint.x, startPoint.y);
}

void TargetPointtCallback(const geometry_msgs::PoseStamped& msg)
{
    Point2d src_point = Point2d(msg.pose.position.x, msg.pose.position.y);
    OccGridParam.Map2ImageTransform(src_point, targetPoint);

    // Set flag
    targetpoint_flag = true;
    if(map_flag && startpoint_flag && targetpoint_flag)
    {
        start_flag = true;
    }

//    ROS_INFO("targetPoint: %f %f %d %d", msg.pose.position.x, msg.pose.position.y,
//             targetPoint.x, targetPoint.y);
}

//-------------------------------- Main function ---------------------------------//
int main(int argc, char * argv[])
{
    //  Initial node
    ros::init(argc, argv, "astar");
    ros::NodeHandle nh;
    ros::NodeHandle nh_priv("~");
    ROS_INFO("Start astar node!\n");

    // Initial variables
    map_flag = false;
    startpoint_flag = false;
    targetpoint_flag = false;
    start_flag = false;

    // Parameter
    nh_priv.param<bool>("Euclidean", config.Euclidean, true);
    nh_priv.param<int>("OccupyThresh", config.OccupyThresh, -1);
    nh_priv.param<double>("InflateRadius", InflateRadius, -1);
    nh_priv.param<int>("rate", rate, 10);

    // Subscribe topics
    map_sub = nh.subscribe("map", 10, MapCallback);
    startPoint_sub = nh.subscribe("initialpose", 10, StartPointCallback);
    targetPoint_sub = nh.subscribe("move_base_simple/goal", 10, TargetPointtCallback);

    // Advertise topics
    mask_pub = nh.advertise<nav_msgs::OccupancyGrid>("mask", 1);
    path_pub = nh.advertise<nav_msgs::Path>("nav_path", 10);

    // Loop and wait for callback
    ros::Rate loop_rate(rate);
    while(ros::ok())
    {
        if(start_flag)
        {
            double start_time = ros::Time::now().toSec();
            // Start planning path
            vector<Point> PathList;
            astar.PathPlanning(startPoint, targetPoint, PathList);
            if(!PathList.empty())
            {
                path.header.stamp = ros::Time::now();
                path.header.frame_id = "map";
                path.poses.clear();
                for(int i=0;i<PathList.size();i++)
                {
                    Point2d dst_point;
                    OccGridParam.Image2MapTransform(PathList[i], dst_point);

                    geometry_msgs::PoseStamped pose_stamped;
                    pose_stamped.header.stamp = ros::Time::now();
                    pose_stamped.header.frame_id = "map";
                    pose_stamped.pose.position.x = dst_point.x;
                    pose_stamped.pose.position.y = dst_point.y;
                    pose_stamped.pose.position.z = 0;
                    path.poses.push_back(pose_stamped);
                }
                path_pub.publish(path);
                double end_time = ros::Time::now().toSec();

                ROS_INFO("Find a valid path successfully! Use %f s", end_time - start_time);
            }
            else
            {
                ROS_ERROR("Can not find a valid path");
            }

            // Set flag
            start_flag = false;
        }

        if(map_flag)
        {
            mask_pub.publish(OccGridMask);
        }

        loop_rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

6、节点启动文件astar.launch:

<!-- -*- mode: XML -*- -->
<launch>
    ################ map server ################
    <node pkg="map_server" name="map_server" type="map_server" args="$(find astar_search)/maps/gmapping-005.yaml"/>

    ################ start astar node ################
    <node pkg="astar_search" type="astar_search" name="astar" output="screen">
        <param name="Euclidean" value="true"/>
        <param name="OccupyThresh" value="-1"/>
        <param name="InflateRadius" value="0.25"/>
        <param name="rate" value="10"/>
    </node>
[map_server-2] killing on exit

    ################ start rviz ################
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find astar_search)/rviz/astar.rviz"/>
</launch>

二、使用方法

2.1 话题的订阅

2.1.1 栅格地图消息的订阅

在ROS中，用nav_msgs/OccupancyGrid消息类型来表示二维栅格地图，该数据类型包含了地图的元信息（如分辨率、地图大小等等）以及每个栅格的占用状态信息。

• nav_msgs/OccupancyGrid消息定义如下：

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data

各个字段的意义如下：

header：用于描述这个消息的meta信息，包括时间戳和坐标系。

info：地图的元数据，这是一个nav_msgs/MapMetaData类型，包含地图的分辨率、尺寸以及提供地图的坐标系的原点等信息。

data：实际的地图数据，这是一个一维数组。数组中的每个元素表示对应栅格的占用信息。值为0表示未占用（通常用于表示可行走的区域），值为100表示被占用（通常用于表示障碍物或者禁行的区域），值为-1表示未知。

总的来说，nav_msgs/OccupancyGrid是一种在ROS中表示二维栅格地图的标准类型，广泛应用于地图构建、路径规划和导航等任务。

2.1.2 起点和终点信息的订阅

initialpose： geometry_msgs/PoseWithCovarianceStamped
movebasesimple/goal： geometry_msgs/PoseStamped

2.2 话题的发布

2.2.1 膨胀地图消息的发布

mask：nav_msgs/OccupancyGrid
navpath：navmsgs/Path

2.2.2 计算出的导航路径消息的发布

2.3 参数

2.3.1 h值

Euclidean(bool; 默认值: “true”)：在计算H值时，选择使用欧几里得距离还是曼哈顿距离。

2.3.2 图像二值化的阈值

OccupyThresh(int; 默认值: -1)：图像二值化的阈值。当OccupyThresh小于零时，利用Otsu方法生成阈值。

2.3.3 膨胀半径

InflateRadius(double; 默认值: -1)：InflateRadius是膨胀半径（单位：米）。当InflateRadius小于或等于零时，不进行膨胀操作。

2.3.4 发布mask主题的频率。

rate(int; 默认值: 10)：发布mask主题的频率。

2.4 编译运行以及Rviz可视化

catkin_make
source ./devel/setup.bash 
roslaunch astar_search astar.launch 

三、A*算法原理

1、初始化openlist（待检查的列表）和closelist（已检查的列表），将起点（start）加入openlist中，并找出start周围可移动的栅格（八叉树），计算start到这些周围点的欧式距离g，并将start设置为父节点。

2、在完成了对起点start的检查之后，把start的周围点加入到openlist中，同时把start从openlist中删除，并加入到closelist中。

3、这时openlist中存放的是start的周围点，计算每个周围点的f=g+h，其中g是起点start到当前节点的距离，h是当前节点到终点的距离（曼哈顿距离），找出周围点中f最小的节点n，并对他执行前面同样的检查。

在搜索n的周围点时注意：
①如果周围点在closedlist中，忽略此周围点；
②如果周围点既不在closedlist中，也不在openlist中，计算g、h和f，设置当前节点n为父节点，并将新的周围点加入到openlist中；
③如果周围点在openlist中（表面该邻居已有父节点），计算该父节点经过当前点n再到周围点是否使其能够得到更小的g，如果可以，将该周围点的父节点设置为当前点n，并更新其g和h值。

完成对当前点n的检查之后，将其从openlist中剔除，并加入到closed中。

4、当openlist中出现目标点goal时，找到路径；当openlist中为空时，则无法找到。

四、ROS/C++代码详解

主要对核心代码Astar.cpp文件进行算法的解释。

1. 在Astar.h中的命名空间pathplanning中声明了Astar类，并且声明了各个部分的类函数用于执行A*算法的路径规划。
2. InitAstar() 函数用于初始化A*算法。有两个重载版本，分别接受地图Map和掩膜图像Mask作为输入参数。它们将输入参数保存到类的成员变量中，并对地图进行预处理。程序中给了地图的yaml文件，直接订阅地图消息即可。

    void InitAstar(Mat& _Map, AstarConfig _config = AstarConfig());

    void InitAstar(Mat& _Map, Mat& Mask, AstarConfig _config = AstarConfig());

3. PathPlanning() 函数用于执行路径规划，函数传入起点和终点参数，并调用Findpath() 方法搜索路径，并将结果保存在给定的路径向量path中。

    void PathPlanning(Point _startPoint, Point _targetPoint, vector<Point>& path);

4. DrauPath() 方法用于将路径绘制在地图上。它接受地图 Map 、路径向量 path 、掩膜图像 Mask、绘制路的颜色color 、线条粗细 thickness 和掩膜的颜色 maskcolor 作为输入。它首先检查路径是否为空，如果是，则输出 “Path is empty” 并返回然后，根居掩膜 Mask 将地图的指定区域清空，并将该区域的像素值设为maskcolor。接着，对于路径中的每个点，使用指定的颜色和线条粗细，在地图上绘制一个矩形。

void Astar::DrawPath(Mat& _Map, vector<Point>& path, InputArray Mask, Scalar color,
        int thickness, Scalar maskcolor)

5. MapProces5() 方法用于对地图进行预处理。它接受图像 Mask 作为输入，并对地图进行转换、二值化、膨胀等操作。首先，将地图转换为灰度图像。然后，根据配置参数 OccupyThresh 的值，对灰度图像进行二值化处理如果 OccupyThresh 小于 0，使用Otsu 值法自动计算闻值，否则使用 OccupyThresh 作为闻值，接下来，如果配置参数 InflateRadius 大于0，则对二值图像进行膨胀操作。通过执行按位异或运算，计算掩膜== Mask== ，将膨胀后的图像和原始图像之间的差异保存在 Mask 中。最后，根据二值图像初始化标签地图 LabelMap，遍历图像的每个像素，将障碍物标记为 obstacle ，自由区域标记为 free 。

    void MapProcess(Mat& Mask);

6. FindPath() 方法是A*算法的核心实现。它使用优先队列 openList 和哈希表 penDict 来存储待扩展的节点，并使用标签地图LabelMap 来记录节点的状态。算法从起点开始，重复以下步骤直到找到目标点或无法找到有效路径:

    Node* FindPath();

• 从 openList 中选择具有最小F值的节点，并将其从队列中移除。
  <1> 根据当前节点的位置获取相邻节点，并根据 A* 算法的启发函数计算每个相邻节点的G 、H 和 F值。
  <2> 如果相邻节点不在 openList 中，将其添加到队列中，并更新 openDict 和 LabelMap。
  <3> 如果相邻节点已经在 openList 中，并且新的G值比旧的G值小，更新相邻节点的 G、F 值和父节点。

7. GetPath() 方法用于获取最终的路径。它从目标节点开始，沿着父节点指针遍历路径，并将节点保存在 Pathlist 中。然后，将PathList 中的节点转换为点坐标，并保存在给定的路径向量 path 中。
8. 在类的私有部分，定义了一些辅助函数和变量，包括 point2index() 函数用于将二维坐标映射为一维索引，Node 结构体表示图节点，以及一些常量和成员变量用于存储地图、配置和算法状态。

总体来说，这段代码实现了A算法的路径规划过程，并提供了一些辅助函数用于地图处理和路径给制，它将地图处理为二值图像，小搜索节点的方式找到最短路径，并将路径绘制在地图上，这段代码的关键在于A算法的实现，它通过优先队列和哈希表来高效地管理节点，并利用启发函数来指导搜索过程。