ROS实现A*算法_a*算法ros

简介

• 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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

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));
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

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();
}

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241

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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201

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>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

二、使用方法

2.1 话题的订阅

2.1.1 栅格地图消息的订阅

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

• nav_msgs/OccupancyGrid消息定义如下：

std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data
1
2
3

各个字段的意义如下：

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 
1
2
3

三、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());
1

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

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

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

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)
1
2

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

    void MapProcess(Mat& Mask);
1

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

    Node* FindPath();
1

• 从 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算法的实现，它通过优先队列和哈希表来高效地管理节点，并利用启发函数来指导搜索过程。