Path planning algorithm - Astar C + + implementation and display
The following are original contents. Please reprint them from famous sources. Thank you.
1. Main function
main.cpp
The main function mainly provides the external interface to implement the algorithm. The main process here is
1. Call MapData(mapPath) to read map data
2. Call Astar Pathpoint() for Astar path lookup
3. The display (astarpath, MapData, xstart, ystart, xstop, ystop, "Astar", true) function displays the final path visually
/*
@Function:Astar External Implementation
@Time:2022-01-10
@Author:Tang Gu Jie
@E-Mail:2822902808@qq.com
*/
#include <iostream>
#include <vector>
#include <string>
#include "Astar.h"
#include "map.h"
#include "display.h"
int main()
{
//string mapPath = "map_small.txt"; // Map path
//int xStart = 4;
//int yStart = 0; // starting point
//int xStop = 2;
//int yStop = 6; // End
string mapPath = "map_big.txt"; //Map path
int xStart = 25;
int yStart = 4; //starting point
int xStop = 3;
int yStop = 16; //End
//f=a*g+b*h
float weightA = 1.0; //Weight a
float weightB = 1.0; //Weight b
//Map data
vector<vector<int>> mapData(MapData(mapPath));
/***
*@Function initialization A star algorithm
------------------------------------------------
*@Parametermapdata map data
*@Parameter xStart start point x value
*@Parameter yStart start y value
*@Parameter xStop end x value
*@Parameter yStop end y value
*@Parameter weightA weight a value
*@Parameter weightA weight b value
*@The parameter PathType is the result of path search, which is assigned to NOFINDPATHPOINT during initialization
*@Parameter DistanceType distance type, EUCLIDEAN European distance, Manhattan distance
------------------------------------------------
*@Return value none
*/
ASTAR::CAstar astar(mapData, xStart, yStart, xStop, yStop, weightA, weightB,ASTAR::CAstar::PathType::NOFINDPATHPOINT,ASTAR::CAstar::DistanceType::EUCLIDEAN);
//A star algorithm path finding function
std::vector<std::pair<int, int> >astarPath = astar.PathPoint();
if (astar.m_noPathFlag == ASTAR::CAstar::PathType::NOFINDPATHPOINT)
{
std::cout << "A The star algorithm failed to find the path!!!" << std::endl;
}
else
{
for (int i = 0; i < astarPath.size(); i++)
{
std::cout << astarPath[i].first << "," << astarPath[i].second << std::endl;
}
/***
*@The function displays the path result, blue starting point, red ending point and black obstacle
------------------------------------------------
*@Parametermapdata map data
*@Parameter xStart start point x value
*@Parameter yStart start y value
*@Parameter xStop end x value
*@Parameter yStop end y value
*@Parameter title of "Astar" picture
*@Parameter true whether to save the pic ture, true to save, false not to save
------------------------------------------------
*@Return value none
*/
Display(astarPath, mapData, xStart, yStart, xStop, yStop, "Astar", true);
}
return 0;
}
2. Reading of map data
map.h
/* @Function:Map Data Read @Time:2022-01-10 @Author:Tang Gu Jie @E-Mail:2822902808@qq.com */ #pragma once #ifndef __MAP__ #define __MAP #include <vector> #include<iostream> using namespace std; vector<vector<int>> MapData(std::string _mapPath); #endif
map.cpp
/*This function reads the map_big.txt forms a two-dimensional array num, in which 0 represents the free passage area and 1 represents the obstacle*/
/*
@Function:Map Data Read
@Time:2022-01-10
@Author:Tang Gu Jie
@E-Mail:2822902808@qq.com
*/
#include "map.h"
#include<fstream>
#include<sstream>
vector<vector<int>>MapData(std::string _mapPath)
{
ifstream f;
//f.open("map_media.txt");
f.open(_mapPath);
//f.open("map.txt");
string str;
vector<vector<int> > num;
while (getline(f, str)) //Read 1 line and assign it to the string str
{
istringstream input(str); // c + + style stream input operation
vector<int> tmp;
int a;
while (input >> a) //Input the data of the first row to a one by one through input
tmp.push_back(a);
num.push_back(tmp);
}
return num;
}
The reading of map data mainly adopts the way of C + + file reading stream. 0 represents free passage area, 1 represents obstacles, and the following is the map_big.txt
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
3. Implementation of astar algorithm
Astar.h
In this header file, only the PathPoint() interface is provided externally, which is the entry function of Astar algorithm. Secondly, in this file, the namespace mechanism is used to prevent other path planning algorithms from using the same name in the future. By enumerating the type DistanceType, we have achieved the function of seeing the meaning of the name when passing parameters to the distance function. In addition, we also add weight a and weight b to the constructor, corresponding to the function of the theoretical part f ( n ) = a × g ( n ) + b × g ( n ) f(n)=a \times g(n)+b \times g(n) f(n)=a × g(n)+b × G (n), so as to realize the deformation of Astar algorithm.
/*
@Function:Astar Algorithm Implementation
@Time:2022-01-10
@Author:Tang Gu Jie
@E-Mail:2822902808@qq.com
*/
#pragma once
#ifndef __ASTAR__
#define __ASTAR__
#include <iostream>
#include <vector>
#include <string>
#include <map>
//Namespace ASTAR
namespace ASTAR
{
//Node structure
struct Node
{
int SnodeX; //Node X value
int SnodeY; //Node Y value
int SparentX; //Parent node X value
int SparentY; //Parent node Y value
float Sgn;; //g value of node
float Shn; //h value of node
float Sfn; //f value of node
};
class CAstar
{
public:
//Enumeration type of path type. NOFINDPATHPOINT means no path point is found, and FINDPATHPOINT means no path point is found
enum PathType {NOFINDPATHPOINT=0, FINDPATHPOINT =1};
//Distance type EUCLIDEAN European distance, Manhattan distance
enum DistanceType {EUCLIDEAN=0,MANHANTTAN=1};
PathType m_noPathFlag;
public:
/*Initialization function*/
CAstar(std::vector<std::vector<int>>_mapData, //Map data, stored as 2D data
int _xStart, //Starting point X value
int _yStart, //Starting point Y value
int _xStop, //Target point X value
int _yStop, //Y value of target point
float _weightA, //Weight a value
float _weightB, //Weight b value
PathType _noPathFlag, //Path (generate flag)
DistanceType _distanceType， //Distance type
);
/*Generate path points*/
std::vector<std::pair<int, int>> PathPoint();
private:
/*Euclidean distance between two points*/
//float EuclideanDistance(int _xNode, int _yNode, int _xTarget, int _yTarget);
/*Insert data into OpenList*/
Node InsertOpen(int _xVal, int _yVal, int _parentXval, int _parentYval, float _hn, float _gn, float _fn);
/*Expand the neighbors around a node*/
std::vector<Node> ExpandArray(int _xNodeExpanded, int _yNodeExpanded, float _gNodeExpanded, int _xTarget, int _yTarget,
std::vector<std::vector<int>> _mapData, std::vector<Node>_closeList);
/*The index of the node in openlist -- > Multimap*/
float NodeIndex(std::multimap<float, Node> _openList, int _xNode, int _yNode);
/*The index of the node in the Closelist -- > vector*/
int CloseNodeIndex(std::vector<Node> _closeList, int _xNode, int _yNode);
/*Astar Core function of*/
void AstarCoreFunction();
/*Get the path of star A algorithm*/
std::vector<std::pair<int, int>> FindPath(std::vector<Node>_closeList, int _xStart, int _yStart, int _xStop, int _yStop);
private:
std::multimap<float, Node> m_openList; //openList list, multimap type (the default Key is sorted from small to large)
std::vector<Node> m_closeList; //closeList list, vector list
std::vector<std::vector<int>> m_mapData; //Map data, double vector type
float m_weightA; //Weight a
float m_weightB; //Weight b
int m_xStart; //Starting point x value
int m_yStart; //Starting point y value
int m_xStop; //End point x value
int m_yStop; //End point y value
};
}
#endif
Astar.cpp
/*
@Function:Astar Algorithm Implementation
@Time:2022-01-10
@Author:Tang Gu Jie
@E-Mail:2822902808@qq.com
*/
#include "Astar.h"
#include <functional>
std::function<float(int, int, int, int)>Distance;
/***
*@The function calculates the Euclidean distance between two points
------------------------------------------------
*@Parameters_ The x value of the xNode node
*@Parameters_ y value of yNode node
*@Parameters_ y value of the xTarget endpoint
*@Parameters_ y value of yTarget endpoint
------------------------------------------------
*@Return value Euclidean distance value: root sign √ ((x2-x1)^2+(y2-y1)^2)
*/
float EuclideanDistance(int _xNode, int _yNode, int _xTarget, int _yTarget)
{
float d = sqrt(pow(_xNode - _xTarget, 2) + pow(_yNode - _yTarget, 2));
return d;
}
/***
*@The function calculates the Manhattan distance between two points
------------------------------------------------
*@Parameters_ The x value of the xNode node
*@Parameters_ y value of yNode node
*@Parameters_ y value of the xTarget endpoint
*@Parameters_ y value of yTarget endpoint
------------------------------------------------
*@Return value: Manhattan distance value: | (x2-x1)+(y2-y1)|
*/
float ManhattanDistance(int _xNode, int _yNode, int _xTarget, int _yTarget)
{
float d = std::abs(_xNode - _xTarget) + std::abs(_yNode - _yTarget);
return d;
}
ASTAR::CAstar::CAstar(std::vector<std::vector<int>>_mapData, //Map data, stored as 2D data
int _xStart, //Starting point X value
int _yStart, //Starting point Y value
int _xStop, //Target point X value
int _yStop, //Y value of target point
float _weightA, //Weight a value
float _weightB, //Weight b value
PathType _noPathFlag, //Path (generate flag)
DistanceType _distanceType //Distance type
) :m_mapData(_mapData), m_xStart(_xStart), m_yStart(_yStart),m_xStop(_xStop), m_yStop(_yStop), m_noPathFlag(_noPathFlag),m_weightA(_weightA),m_weightB(_weightB)
{
switch (_distanceType)
{
case ASTAR::CAstar::EUCLIDEAN:
{
std::cout << "Use European distance!!!" << std::endl;
Distance = EuclideanDistance;
break;
}
case ASTAR::CAstar::MANHANTTAN:
{
std::cout << "Use Manhattan distance!!!" << std::endl;
Distance = ManhattanDistance;
break;
}
default:
break;
}
}
/***
*@The function function inserts a new node into openList
------------------------------------------------
*@Parameters_ xVal the x value of the current node
*@Parameters_ yVal the y value of the current node
*@Parameters_ parentXval the x value of the parent node of the current node
*@Parameters_ parentYval the y value of the parent node of the current node
*@Parameters_ hn h value of the current node (relationship between the current node and the end point)
*@Parameters_ gn the g value of the current node (the relationship between the current node and the previous one)
*@Parameters_ fn f value of current node (f=g+h)
------------------------------------------------
*@The return value Node is the Node assigned with the above parameters
*/
ASTAR::Node ASTAR::CAstar::InsertOpen(int _xVal, int _yVal, int _parentXval, int _parentYval, float _hn, float _gn, float _fn)
{
Node node;
node.SnodeX = _xVal;
node.SnodeY = _yVal;
node.SparentX = _parentXval;
node.SparentY = _parentYval;
node.Shn = _hn;
node.Sgn = _gn;
node.Sfn = _fn;
return node;
}
/***
*@The function extends a node's surrounding neighbor nodes
------------------------------------------------
*@Parameters_ xNodeExpanded the node x value of the neighbor to be extended
*@Parameters_ yNodeExpanded node y value of the neighbor to be extended
*@Parameters_ gNodeExpanded the node g value of the neighbor to be extended
*@Parameters_ xTarget end x value
*@Parameters_ yTarget end y value
*@Parameters_ mapData map data
*@Parameters_ closeList close list
------------------------------------------------
*@Return value none
*/
std::vector<ASTAR::Node> ASTAR::CAstar::ExpandArray(int _xNodeExpanded, int _yNodeExpanded, float _gNodeExpanded, int _xTarget, int _yTarget,std::vector<std::vector<int>> _mapData, std::vector<ASTAR::Node>_closeList)
{
//The 8 domains of the node are in the order of lower right, lower left, lower left, right, left, upper right, upper left
int mapHeight = _mapData.size();
int mapWidth = _mapData[1].size();
Node node;
std::vector<Node> nodeList;
bool expandFlag = false;
for (int k = 1; k >= -1; k--)
{
for (int j = 1; j >= -1; j--)
{
if (k != j || j != 0)
{
int sx = _xNodeExpanded + k;
int sy = _yNodeExpanded + j;
if ((sx >= 0 && sx < mapHeight) && (sy >= 0 && sy < mapWidth) && _mapData[sx][sy] != 1)
{
expandFlag = true;
for (int i = 0; i < _closeList.size(); i++)
{
if (sx == _closeList[i].SnodeX && sy == _closeList[i].SnodeY)
{
expandFlag = false;
break;
}
}
if (expandFlag == true)
{
node.SnodeX = sx;
node.SnodeY = sy;
node.SparentX = -1; //Just fill in this
node.SparentY = -1; //Just fill in this
node.Sgn = (_gNodeExpanded + EuclideanDistance(_xNodeExpanded, _yNodeExpanded, sx, sy));
node.Shn = (Distance(_xTarget, _yTarget, sx, sy));
node.Sfn = m_weightA*node.Sgn + m_weightB*node.Shn;
nodeList.emplace_back(node);
}
}
}
}
}
return nodeList;
}
/***
*@Function to query the Key index of the node (x,y) in openList
------------------------------------------------
*@Parameters_ openList open list
*@Parameters_ xNode node x value
*@Parameters_ yNode node y value
------------------------------------------------
*@The return value is the Key value index. Note that in the multimap, the Key may be duplicate, - 1.0 means it is not found
*/
float ASTAR::CAstar::NodeIndex(std::multimap<float, Node> _openList, int _xNode, int _yNode)
{
for (std::multimap<float, Node>::iterator it = _openList.begin(); it != _openList.end(); it++)
{
if (it->second.SnodeX == _xNode && it->second.SnodeY == _yNode)
{
return it->first;
}
}
return -1.0;
}
/***
*@Function to query the index of the node (x,y) in the closeList
------------------------------------------------
*@Parameters_ closeList close list
*@Parameters_ xNode node x value
*@Parameters_ yNode node y value
------------------------------------------------
*@The return value i indicates the index of the node in the vector, - 1 indicates that it is not found
*/
int ASTAR::CAstar::CloseNodeIndex(std::vector<Node> _closeList, int _xNode, int _yNode)
{
for (int i = 0; i < _closeList.size(); i++)
{
if (_closeList[i].SnodeX == _xNode && _closeList[i].SnodeY == _yNode)
{
return i;
}
}
return -1;
}
/***
*@Function function is the core function of Astar. All data processing is completed here
------------------------------------------------
*@Parameter none
*@Parameter none
------------------------------------------------
*@The return value is none. In fact, there is a hidden output closeList. After the final function is completed, the closeList will be improved. Finally, the path points will be inverted according to the closeList
*/
void ASTAR::CAstar::AstarCoreFunction()
{
float goalDistance = EuclideanDistance(m_xStart, m_yStart, m_xStop, m_yStop);
float pathCost =0.0;
float f_ = m_weightA*goalDistance + m_weightB*pathCost;
//Put the starting point into the openList
Node node = InsertOpen(m_xStart, m_yStart, m_xStart, m_yStart, goalDistance, pathCost, f_);
m_openList.insert(std::make_pair(goalDistance, node));
while (true)
{
//Corresponding pseudo code -- > if the queue (openlist) is empty, return false; break;
if (m_openList.size() == 0)
break;
//Corresponding pseudo code -- > remove the node "n" with the lowest f (n) from the priority queue
std::multimap<float, Node>::iterator pos = m_openList.begin(); //The multimap key values are arranged from small to large by default
int xNodeExpanded = pos->second.SnodeX;
int yNodeExpanded = pos->second.SnodeY;
float gNodeExpanded = pos->second.Sgn;
// Corresponding pseudo code -- > mark node "n" as expanded
Node closeNode = pos->second;
m_closeList.emplace_back(closeNode);
m_openList.erase(pos);
//Corresponding pseudo code -- > if the node "n" is the goal state, return true; break;
if (xNodeExpanded == m_xStop && yNodeExpanded == m_yStop)
{
m_noPathFlag = FINDPATHPOINT;
break;
}
//Get all "available" neighbor sets for all nodes n
std::vector<Node> nodeList = ExpandArray(xNodeExpanded, yNodeExpanded, gNodeExpanded, m_xStop, m_yStop, m_mapData, m_closeList);
for (int i = 0; i < nodeList.size(); i++)
{
int xNode = nodeList[i].SnodeX;
int yNode = nodeList[i].SnodeY;
float nodeIndex = NodeIndex(m_openList, xNode, yNode);
if (nodeIndex == -1.0)
{
//Corresponding pseudo code -- > If node m is not in openlist, push node m into openlist
float gn = gNodeExpanded + EuclideanDistance(xNode, yNode, xNodeExpanded, yNodeExpanded);
float hn = Distance(xNode, yNode, m_xStop, m_yStop);
float fn = m_weightA*gn+ m_weightB*hn;
Node node_ = InsertOpen(xNode, yNode, xNodeExpanded, yNodeExpanded, hn, gn, fn);
m_openList.insert(std::make_pair(fn, node_));
}
else
{
//Corresponding pseudo code -- > if G (m) > G (n) + CNM
//lower_bound returns the first iterator of the search result, upper_bound returns the iterator at the next position of the last lookup result
std::multimap<float, Node>::iterator indexLow = m_openList.lower_bound(nodeIndex);
std::multimap<float, Node>::iterator indexUpper = m_openList.upper_bound(nodeIndex);
indexUpper--;
if (indexLow == indexUpper)
{
//Indicates that there are no duplicate key values
if (indexLow->second.Sgn > (gNodeExpanded + EuclideanDistance(xNode, yNode, xNodeExpanded, yNodeExpanded)))
{
indexLow->second.SparentX = xNodeExpanded;
indexLow->second.SparentY = yNodeExpanded;
indexLow->second.Sgn = gNodeExpanded + EuclideanDistance(xNode, yNode, xNodeExpanded, yNodeExpanded);
indexLow->second.Sfn = m_weightA*indexLow->second.Sgn + m_weightB*indexLow->second.Shn;
}
}
else
{
//Indicates that there are duplicate key values
while (indexLow != indexUpper)
{
if (indexLow->second.SnodeX == xNode && indexLow->second.SnodeY == yNode)
break;
indexLow++;
}
if (indexLow->second.Sfn > (gNodeExpanded + EuclideanDistance(xNode, yNode, xNodeExpanded, yNodeExpanded)))
{
indexLow->second.SparentX = xNodeExpanded;
indexLow->second.SparentY = yNodeExpanded;
indexLow->second.Sgn = gNodeExpanded + EuclideanDistance(xNode, yNode, xNodeExpanded, yNodeExpanded);
indexLow->second.Sfn = m_weightA*indexLow->second.Sgn + m_weightB*indexLow->second.Shn;
}
}
}
}
}
}
/***
*@Function function path finding function
------------------------------------------------
*@Parameters_ closeList close list
*@Parameters_ xStart start X value
*@Parameters_ yStart start Y value
*@Parameters_ xStop end X value
*@Parameters_ yStop end Y value
------------------------------------------------
*@Return value path node type [(first,second), (), ()...]
*/
std::vector<std::pair<int, int>> ASTAR::CAstar::FindPath(std::vector<Node>_closeList, int _xStart, int _yStart, int _xStop, int _yStop)
{
std::pair<int, int>path;
std::vector<std::pair<int, int>>findPath;
path.first = _xStop;
path.second = _yStop;
findPath.emplace_back(path);
int index = CloseNodeIndex(_closeList, _xStop, _yStop);
while (true)
{
if (_closeList[index].SparentX == _xStart && _closeList[index].SparentY == _yStart)
{
break;
}
int nodeX = _closeList[index].SparentX;
int nodeY = _closeList[index].SparentY;
path.first = nodeX;
path.second = nodeY;
findPath.emplace_back(path);
index = CloseNodeIndex(_closeList, nodeX, nodeY);
}
return findPath;
}
/***
*@Function function interface to external A-star algorithm
------------------------------------------------
*@Parameter none
------------------------------------------------
*@Return value path node type [(first,second), (), ()...]
*/
std::vector<std::pair<int, int>> ASTAR::CAstar::PathPoint()
{
//AstarCoreFunction improves openList and closeList
AstarCoreFunction();
//FindPath finds path points through closeList
std::vector<std::pair<int, int> > pathPoint = FindPath(m_closeList, m_xStart, m_yStart, m_xStop, m_yStop);
return pathPoint;
}
Special instructions are as follows:
- Using the function wrapper function in the constructor, you can bind a function with another function during compilation (of course, this is not particularly accurate, you can understand it). In this way, you can call different functions as long as you pass different parameters through the function** In fact, the simplest way to achieve the above functions is to set a flag bit in the constructor and then add another function f f f calls different functions through different flag bits. In this way, the above functions can be realized, but each call f f f function will judge a flag bit, which is not particularly good. Here is a pseudo code written by yourself, which I hope will help you understand** Therefore, a function wrapper is generated. This is equivalent to writing many functions. Different functions can be bound dynamically through different constructor parameters.
"Header file"
class A
{
...
public:
A(string a)
{
switch(a)
{
case "a":
flag1=true;
break;
case "b":
flag2=true;
break;
default:
break;
}
}
public:
bool flag1=false;
bool flag2=false;
....
};
"source file"
void test1()
{
...
}
void test2()
{
...
}
void f()
{
if (flag1==true)
test1();
else if(flag2==true)
test2();
...
}
-
openLIst is designed using std::multimap to f f f as the key, N o d e Node Node is the value, so you can skillfully use multimap. By default, it is sorted according to the key value (from small to large by default). You must use an iterator to take the first element of the multimap, which is f f f the smallest node. The advantage of this is that when the node is inserted, the node already follows the key value f f f is sorted.
-
As for the choice of function distance, in fact, no matter the European distance or Manhattan distance, what changes is the estimated cost** h ( n ) h(n) h(n) * *, for g ( n ) g(n) g(n) still adopts European distance. You can refer to Dijkstra's method. Please note that I was stuck here for a long time, and the simulation effect is particularly strange; For the change of weights a and b, only f ( n ) f(n) The value of f(n) does not change g ( n ) g(n) g(n) and h ( n ) h(n) h(n).
-
The neighbor node of a node uses the method of 8 fields, which is lower right, lower and sit down; Right, left; Top right, top left.
-
The running part of the function is PathPoint → \to → AstarCoreFunction+FindPath, the specific function has been marked in the program, and you can understand it by yourself.
4. Realization of display function
display.h
/* @Function:Astar Algorithm Display @Time:2022-01-10 @Author:Tang Gu Jie @E-Mail:2822902808@qq.com */ #pragma once #ifndef __DISPLAY__ #define __DISPLAY__ #include<vector> #include<iostream> #include<opencv2/opencv.hpp> #include <string> #define Size 30 #define Menu Size/2 #define Width 1200 #define Height 1200 using namespace std; void Display(vector<pair<int,int> >_pathPoint,vector<vector<int>> _mapData, int _xStart, int _yStart, int _xStop, int _yStop,string _pictureName,bool _saveFlag); #endif
display.cpp
/*
@Function:Astar Algorithm Display
@Time:2022-01-10
@Author:Tang Gu Jie
@E-Mail:2822902808@qq.com
*/
#include "display.h"
//#include "fit.h"
void Display(vector<pair<int, int> >_pathPoint, vector<vector<int>>_mapData,int _xStart,int _yStart,int _xStop,int _yStop,string _pictureName,bool _saveFlag)
{
int ROWS = _mapData.size();
int COLS = _mapData[1].size();
cv::Mat img(Height, Width, CV_8UC3, cv::Scalar(255, 255, 255));
cv::Point left_up, right_bottom;
cv::Point point_first, point_second;
//Middle path point -- > yellow
for (int i = 0; i < _pathPoint.size(); i++)
{
left_up.x = _pathPoint[i].second * Size;
left_up.y = _pathPoint[i].first * Size;
right_bottom.x = left_up.x + Size;
right_bottom.y = left_up.y + Size;
cv::rectangle(img, left_up, right_bottom, cv::Scalar(0, 255, 255), CV_FILLED, 8, 0);//path yellow(full)
}
//Obstacle -- > black, starting point -- > blue, ending point -- > Red
for (int i = 0; i<ROWS; i++)
{
for (int j = 0; j<COLS; j++)
{
left_up.x = j * Size; //The column (j) storing the array corresponds to the x-axis of the rectangle
left_up.y = i * Size;
right_bottom.x = left_up.x + Size;
right_bottom.y = left_up.y + Size;
if (_mapData[i][j])
{
cv::rectangle(img, left_up, right_bottom, cv::Scalar(0, 0, 0), CV_FILLED, 8, 0);//obstacles balck
}
else
{
if (i == _xStart&&j == _yStart)
cv::rectangle(img, left_up, right_bottom, cv::Scalar(255, 0, 0), CV_FILLED, 8, 0);//start point blue(full)
else if (i == _xStop&&j == _yStop)
cv::rectangle(img, left_up, right_bottom, cv::Scalar(0, 0, 255), CV_FILLED, 8, 0);//goal point red(full)
}
}
}
//Middle line -- > yellow
for (int i = 1; i < COLS; i++)
{
point_first.x = i * Size;
point_first.y = 1 * Size;
point_second.x = i *Size;
point_second.y = (ROWS - 1) * Size;
cv::line(img, point_first, point_second, cv::Scalar(141,238,238), 2, 2);
}
for (int i = 1; i < ROWS;i++)
{
point_first.x = 1 * Size;
point_first.y = i * Size;
point_second.x = (COLS - 1) * Size;
point_second.y = i * Size;
cv::line(img, point_first, point_second, cv::Scalar(141,238,238), 2, 2);
}
//Path line -- > yellow
point_first.x = _yStop * Size + Menu;
point_first.y = _xStop * Size + Menu;
for (int i = 0; i < _pathPoint.size(); i++)
{
left_up.x = _pathPoint[i].second * Size;
left_up.y = _pathPoint[i].first * Size;
point_second.x = left_up.x + Menu;
point_second.y = left_up.y + Menu;
cv::line(img, point_first, point_second, cv::Scalar(0, 0, 0), 2, 4);
point_first = point_second;
}
if (_saveFlag)
{
string str1 = ".png";
_pictureName.append(str1);
cv::imwrite(_pictureName, img);
cout << "save png success" << endl;
}
cv::imshow(_pictureName, img);
cv::waitKey(0);
}
The implementation of display is based on opencv3 4, just pass in the path node. The following display in C + + is not as convenient as matlab or python. This is written by an online blogger. The bottom layer of the plug-in for path planning under ROS is implemented in C + +. In fact, it still needs to be implemented in C + +.
5. Algorithm simulation
Blue represents the starting point, red represents the end point, black is the obstacle, white is the passable part, and the black line is the path obtained by Astar.
Simulation one
Astar
|
Dijkstra
|
Greedy
|
Weight Astar(a=1.0,b=1.1)
|
Simulation II
|
Astar Euclidean distance
|
Astar Manhattan distance
|
The above codes are slowly debugged by ourselves. There may be some problems. Add me QQ: 2822902808 to find the problem. Let's exchange progress together. Thank you.
For the theory part, refer to my last blog: (1) Path planning algorithm Graph Search Basis