ROS实现A*算法

ROS实现A*算法

• 简介
• 一、ROS/C++代码
• • 1、栅格地图头文件OccMapTransform.h：
  • 2、栅格地图源文件OccMapTransform.cpp：
  • 3、Astar.h:
  • 4、Astar.cpp:
  • 5、主函数main.cpp:
  • 6、节点启动文件astar.launch:
• 二、使用方法
• • 2.1 话题的订阅
  • • 2.1.1 栅格地图消息的订阅
    • 2.1.2 起点和终点信息的订阅
  • 2.2 话题的发布
  • • 2.2.1 膨胀地图消息的发布
    • 2.2.2 计算出的导航路径消息的发布
  • 2.3 参数
  • • 2.3.1 h值
    • 2.3.2 图像二值化的阈值
    • 2.3.3 膨胀半径
    • 2.3.4 发布mask主题的频率。
  • 2.4 编译运行以及Rviz可视化
• 三、A*算法原理
• 四、ROS/C++代码详解

简介

• 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 
#include 
#include 

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(0, 0) = resolution * cos(theta);
    R.at(0, 1) = resolution * sin(-theta);
    R.at(1, 0) = resolution * sin(theta);
    R.at(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(0, 0);
    dst_point.y = P_dst.at(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(0, 0));
    dst_point.y = height - 1 - round(P_dst.at(1, 0));
}

3、Astar.h:

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

#ifndef ASTAR_H
#define ASTAR_H

#include 
#include 
#include 
#include 

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 a, pair 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& path);
    void DrawPath(Mat& _Map, vector& 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& path);

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

    Mat LabelMap;
    AstarConfig config;

    priority_queue<pair, vector<pair>, cmp> OpenList; // open list
    unordered_map OpenDict; // open dict
    vector 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& path)
{
    // Get variables
    startPoint = _startPoint;
    targetPoint = _targetPoint;

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

void Astar::DrawPath(Mat& _Map, vector& 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(y, x) == 0)
            {
                LabelMap.at(y, x) = obstacle;
            }
            else
            {
                LabelMap.at(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(startPointNode->F, startPointNode->point));
    int index = point2index(startPointNode->point);
    OpenDict[index] = startPointNode;
    _LabelMap.at(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(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(k, 0);
            int x = curX + neighbor.at(k, 1);
            if(x = width || y = height)
            {
                continue;
            }
            if(_LabelMap.at(y, x) == free || _LabelMap.at(y, x) == inOpenList)
            {
                // Determine whether a diagonal line can pass
                int dist1 = abs(neighbor.at(k, 0)) + abs(neighbor.at(k, 1));
                if(dist1 == 2 && _LabelMap.at(y, curX) == obstacle && _LabelMap.at(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(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(node->F, node->point));
                    int index = point2index(node->point);
                    OpenDict[index] = node;
                    _LabelMap.at(y, x) = inOpenList;
                }
                else // _LabelMap.at(y, x) == inOpenList
                {
                    // Find the node
                    int index = point2index(Point(x, y));
                    Node* node = OpenDict[index];
                    if(G G)
                    {
                        node->G = G;
                        node->F = F;
                        node->parent = CurNode;
                    }
                }
            }
        }
    }

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

void Astar::GetPath(Node* TailNode, vector& 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
    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 
#include 
#include 

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

    ################ map server ################
    

    ################ start astar node ################
    
        
        
        
        
    
[map_server-2] killing on exit

    ################ start rviz ################
    

二、使用方法

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& path);

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

void Astar::DrawPath(Mat& _Map, vector& 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值的节点，并将其从队列中移除。

  根据当前节点的位置获取相邻节点，并根据 A* 算法的启发函数计算每个相邻节点的G 、H 和 F值。

  如果相邻节点不在 openList 中，将其添加到队列中，并更新 openDict 和 LabelMap。

  如果相邻节点已经在 openList 中，并且新的G值比旧的G值小，更新相邻节点的 G、F 值和父节点。

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

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