无人机激光雷达的路径规划仿真_a*实现3d激光雷达路径规划

项目链接：gitee-飞行器信息系统课程设计
这个题目的说明：无人机路径规划

无人机从红色的起点出发，朝向蓝色的终点飞行，但是事先不知道环境地图（障碍物）的分布。

假设无人机上装备了激光雷达，能够探测一定范围内的障碍物信息

• 当前探测到的障碍物用红色显示
• 还未探测到的障碍物用灰色显示
• 已经探测到的障碍物用黑色显示

无人机利用A * 算法规划当前认为可行的路径，绿色的线段

但是随着探测范围扩大，无人机在飞行过程发现，利用之前规划的路线无法达到目标点，则需要重新规划路径

利用上述方法，不断探测障碍物，并不断规划路径，无人机最终到达目标点

详细的每一步需要参考每个文件夹的README说明文档；

这里只放出最后的总结；

1 动态地图

即根据自身位置，设置无人机只知道自身某半径范围内的所有地图

2 动态规划

由于截至12-10日，雷达的仿真已经实现，现可以开始最后一步的设计，即动态规划；

需要详细考虑一下数据流

• 首先，无人机位于起始位置，该点已知；将该点的pos传入雷达地图获取函数；
• 生成一定半径以内探测到的障碍物grid，应该有两个grid，一个为0-1用于A*，一个保留0-2便于生成图片
• grid和pos用于A*算法，grid应是一个建图的过程；
• 在每一次建图完成后，跑一次A*，把Path入栈保存；在这次跑完的A star的Path中，断不会有与障碍物重叠的点；
• 对于pos路径的下一个节点，就是存在Path中的节点，从pos往下的一个，作为pos传给雷达扫描建图函数，更新0-1数组地图；
• 对于Path中当前点到终点，判断是否有的点在地图上为障碍物了，如果一旦出现，以当前位置为起始位置，再次开始A star，Path入stack

基本是这样；

3 结果

4 Header

//
// Created by hazyparker on 2021/12/10.
//

#ifndef INC_5_FINAL_SOLUTION_H
#define INC_5_FINAL_SOLUTION_H
#include "cmath"
#include <array>
#include <chrono>
#include <cstring>
#include <iostream>
#include <queue>
#include <set>
#include <stack>
#include <tuple>
#include <vector>
#include <utility>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

typedef pair<int, int> Pair;
typedef vector<vector<int>> Vec2int;
typedef tuple<bool, int, int, int> PathVar;

class Solution {
private:
    int ROW;
    int COL;

    Pair StartPoint;
    Pair EndPoint;
    stack<Pair> DynamicPath;
    // FIXME: cannot define vector in class? like vector<int> vec; have to be vec = vector<int>(100)
    // reference:
    // https://stackoverflow.com/questions/70332332/how-to-use-vector-as-a-private-member-in-class-to-store-and-pass-data/70332667#70332667
    // however, it's still not fixed

    Vec2int map_vector;
    Vec2int radar_vector;
    Vec2int check_vector;
    Vec2int show_vector;

public:
    Solution():
            ROW{ 100 },
            COL{ 100 },
            map_vector(ROW, vector<int>(COL)),
            radar_vector(ROW, vector<int>(COL)),
            check_vector(ROW, vector<int>(COL)),
            show_vector(ROW, vector<int>(COL)),
            StartPoint({10, 10}),
            EndPoint({90, 90})
    {}
    // A structure to hold the necessary parameters
    struct cell {
        // Row and Column index of its parent
        Pair nodeParent;
        // f = g + h
        double cost_f, cost_g, cost_h;
        cell():
                nodeParent(-1, -1), cost_f(-1), cost_g(-1), cost_h(-1){}
    };


    /**
     * set start point in coordinate position, then trans to array pos
     * @param x corPos x
     * @param y corPos y
     */
    void setStartPoint(int x, int y);

    /**
     * set end point in coordinate position, then trans to array pos
     * @param x corPos x
     * @param y corPos y
     */
    void setEndPoint(int x, int y);

    /**
     * transfer coordinate position to (i, j) display in array which starting at [0]
     * @param corPos pair(STL, C++) coordinate position of a point
     * @return array position
     */
    static Pair cor2ArrayPos(Pair corPos);

    /**
     * check whether given cell (row, col) is a valid cell or not.
     * @param grid
     * @param point
     * @return bool
     */
    bool isValid(const vector<vector <int>> &grid, const Pair& point) const;

    /**
     * A Utility Function to check whether the given cell is blocked or not
     * @param grid
     * @param point
     * @return
     */
    bool isUnBlocked(const vector<vector <int>> &grid, const Pair& point) const;

    /**
     * A Utility Function to check whether destination cell has been reached or not
     * @param position
     * @param dest
     * @return
     */
    bool isDestination(const Pair& position);

    /**
     * A Utility Function to calculate the 'h' heuristics
     * @param src
     * @param dest
     * @return
     */
    static double calculateHValue(const Pair& src, const Pair& dest);

    /**
     * transfer vector<vector<int>> to gray scale Mat(image)
     * @param grid
     * @return Mat mat
     */
    Mat vector2ObstacleMap(Vec2int &grid) const;

    /**
     * read m_map array from file(mapName), write it in map_vector
     * @param mapName
     * @return map_vector
     */
    Vec2int getVectorRawGrid(const string& mapName);

    /**
     * using erode function in openCV to add boundaries
     * @param grid
     * @return vector<vector<int>>
     */
    Vec2int getVectorBoundaryGrid(Vec2int &grid) const;

    /**
     * transfer cv::Mat to vector<vector<int>>
     * @param src
     * @param m
     * @param n
     * @return
     */
    static vector<vector<int>> mat2Vector(Mat &src, int m, int n);

    /**
     * transfer vector<vector<int>> to cv::Mat
     * @param vec
     * @return
     */
    static Mat vector2Mat(vector<vector<int>> &vec);

    /**
     * Lidar Stimulation, fetch data from radar based on position currently
     * @param pos
     * @param Vec
     */
    void fetchLidarData(Pair &pos, Vec2int &Vec);

    /**
     * merge Intervals
     * @param newRange
     * @param intervals
     * @return
     */
    static vector<pair<double, double>> mergeInterval(const pair<double, double> &newRange,
                                               vector<pair<double, double>> &intervals);

    /**
     * check if the interval you input is included in intervals that you check
     * @param newRange
     * @param intervals
     * @return
     */
    static bool checkInterval(const pair<double, double> &newRange,
                                 vector<pair<double, double>> &intervals);

    /**
     * calculate degree range of a RECT
     * assuming radar located at point_0, target pos is point_d
     * @param point_0
     * @param point_d
     * @return
     */
    static pair<double, double> getThetaRange(Pair point_0, Pair point_d);

    /**
     * clear vector maps already scanned
     * cos 1 means visible and reachable, setting it as 1
     */
    void clearRadarMap();

    /**
     * launch radar
     */
    void launchRadar();

    /**
     * A star search while pos and grid is changing
     * @param pos
     * @param m_grid
     */
    void dynamicSearch(Pair &pos, Vec2int &m_grid);

    /**
     * trace path when A star is finished
     * @param cellDetails
     * @param pos
     */
    void dynamicTracePath(const vector<vector <cell>> &cellDetails, const Pair &pos);

    /**
     * check if there is Obstacle on Path calculated by A star
     * @param path
     * @return
     */
    PathVar ifPathObstacle(vector<Pair> &path);

    /**
     * show path using openCV
     */
    void showPath();
};


#endif //INC_5_FINAL_SOLUTION_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
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

5 Main函数

#include "Solution.h"

using namespace std;
using namespace cv;

int main() {
vector<vector<int>> grid;
    Solution solve;
    solve.getVectorRawGrid("test_1.map");
    solve.setStartPoint(40, 10);
    solve.setEndPoint(40, 90);
    solve.launchRadar();
    solve.showPath();

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

6 CMakeList

cmake_minimum_required(VERSION 3.21)
project(5_final)

set(CMAKE_CXX_STANDARD 14)

#  OpenCV
FIND_PACKAGE(OpenCV REQUIRED)
INCLUDE_DIRECTORIES({OpenCV_INCLUDE_DIRS})

add_executable(5_final main.cpp AStarSearch.cpp AStarSearch.h readMap.cpp readMap.h testRadar.cpp testRadar.h Solution.cpp Solution.h)

# link
TARGET_LINK_LIBRARIES(5_final ${OpenCV_LIBS})
1
2
3
4
5
6
7
8
9
10
11
12
13

7 Solution cpp

//
// Created by hazyparker on 2021/12/10.
//

#include "Solution.h"
#include "cmath"
#include <array>
#include <chrono>
#include <cstring>
#include <iostream>
#include <queue>
#include <tuple>
#include <vector>
#include <utility>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

typedef pair<int, int> Pair;
typedef vector<vector<int>> Vec2int;
typedef tuple<double, int, int> Tuple;
typedef tuple<bool, int, int, int> PathVar;

vector<Pair> OverallPath;

Vec2int Solution::getVectorRawGrid(const string& mapName) {
    string FILEPATH = "../../maps/" + mapName;
    const char *filepath = (char*)FILEPATH.data();
    // read map
    int32_t m_mx, m_my, m_startX, m_startY, m_endX, m_endY;
    int8_t *m_map;
    Mat img;

    FILE        *fp;
    uint32_t    f_magic;

    // open file
    fp = fopen(filepath, "rb");
    if( fp == nullptr ) {
        printf("ERR: failed to open file %s!\n", filepath);
        exit(100);
    }

    // check file magic number
    fread(&f_magic, sizeof(uint32_t), 1, fp);
    if( f_magic != 0x15432345 ) {
        printf("ERR: input file format is not correct! %s\n", filepath);
        goto MAP_LOAD_RET;
    }

    // read map nx, ny
    fread(&m_mx, sizeof(int32_t), 1, fp);
    fread(&m_my, sizeof(int32_t), 1, fp);
    ROW = m_mx;
    COL = m_my;

    // read map
    m_map = new int8_t[m_mx * m_my];
    fread(m_map, sizeof(int8_t), m_mx * m_my, fp);

    // read start,end point
    fread(&m_startX, sizeof(int32_t), 1, fp);
    fread(&m_startY, sizeof(int32_t), 1, fp);
    fread(&m_endX, sizeof(int32_t), 1, fp);
    fread(&m_endY, sizeof(int32_t), 1, fp);
    StartPoint = {m_startX, m_startY};
    EndPoint = {m_endX, m_endY};

    MAP_LOAD_RET:
    fclose(fp);

    int cols = m_mx;
    // Mat to Vector2int
    for (int i = 0; i < m_mx; i++){
        for (int j = 0; j < m_my; j++){
            // 1 is reachable
            // 0 is blocked
            if (int(m_map[i * cols + j]) == 0){
                map_vector[i][j] = 1;
            }
            if (int(m_map[i * cols + j]) == 1){
                map_vector[i][j] = 0;
            }
        }
    }

    return map_vector;
}

Pair Solution::cor2ArrayPos(Pair corPos) {
    Pair ArrayPos;
    ArrayPos.first = corPos.second - 1;
    ArrayPos.second = corPos.first - 1;
    return ArrayPos;
}

void Solution::setStartPoint(int x, int y) {
    Pair corPos;
    corPos.first = x;
    corPos.second = y;

    StartPoint = cor2ArrayPos(corPos);
}

void Solution::setEndPoint(int x, int y) {
    Pair corPos;
    corPos.first = x;
    corPos.second = y;

    EndPoint = cor2ArrayPos(corPos);
}

bool Solution::isValid(const vector<vector<int>> &grid, const Pair &point) const {
    // Returns true if row number and column number is in range
    if (ROW > 0 && COL > 0)
        return (point.first >= 0) && (point.first < ROW)
               && (point.second >= 0)
               && (point.second < COL);

    else return false;
}

bool Solution::isUnBlocked(const vector<vector<int>> &grid, const Pair &point) const {
    // Returns true if the cell is not blocked else false
    return isValid(grid, point)
           && grid[point.first][point.second] == 1;
}

bool Solution::isDestination(const Pair &position) {
    return position == EndPoint;
}

double Solution::calculateHValue(const Pair &src, const Pair &dest) {
    // h is estimated with the two points distance formula
    return sqrt(pow((src.first - dest.first), 2.0)
                + pow((src.second - dest.second), 2.0));
}

Mat Solution::vector2ObstacleMap(Vec2int &grid) const {
    Mat img, dst;
    img = Mat(ROW, COL, CV_8U, Scalar::all(255));

    // convert m_map to int, sending to img
    for (int i = 0; i < COL; i++){
        for (int j = 0; j < ROW; j++){
            img.at<uchar>(i, j) = grid[i][j] * 255;
        }
    }

    // erode, showing the edge
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    erode(img, dst, element);

    // uplift the gray scale of edges
    for (int i = 0; i < COL; i++){
        for (int j = 0; j < ROW; j++){
            if(int(dst.at<uchar>(i, j)) == 0)
                dst.at<uchar>(i, j) = 200;
        }
    }

    // blend two images, using lower gray scale
    // consider using addWeight()
    for (int i = 0; i < COL; i++){
        for (int j = 0; j < ROW; j++){
            if(int(dst.at<uchar>(i, j)) == 200 && int(img.at<uchar>(i, j)) == 0)
                dst.at<uchar>(i, j) = 0;
        }
    }

    return dst;
}

Vec2int Solution::getVectorBoundaryGrid(Vec2int &grid) const {
    Mat img, dst;
    vector<vector <int>> boundaryGrid(ROW, vector<int>(COL));

    img = vector2Mat(grid);
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    erode(img, dst, element);

    boundaryGrid = mat2Vector(dst, ROW, COL);
    return boundaryGrid;
}

vector<vector <int>> Solution::mat2Vector(Mat &src, int m, int n) {
    vector<vector <int>> transferredVector(m, vector<int>(n));
    vector<int> flatVector = src.reshape(1,1);
    // FIXME: how to transfer Mat to vector<vector>
//    transferredVector = flatVector.resize(m, 0);
    for (int i = 0; i < m; i++){
        for (int j = 0; j < m; j++){
            transferredVector[i][j] = int(src.at<uchar>(i, j));
        }
    }
    return transferredVector;
}

Mat Solution::vector2Mat(vector<vector <int>> &vec) {
    Mat dst;
    dst = Mat(100, 100, CV_8U, Scalar::all(0));
    for (int i = 0; i < 100; i++){
        for (int j = 0; j < 100; j++){
            dst.at<uchar>(i, j) = vec[i][j];
        }
    }
    return dst;
}

void Solution::fetchLidarData(Pair &pos, Vec2int &Vec) {
    // set radius
    int radius = 40;
    int pos_i = pos.first;
    int pos_j = pos.second;

    // open Lidar
    pair<double, double> range;
    vector<pair<double, double>> degreeInterval;
    degreeInterval.emplace_back(0.001, 0.002);
    int i, j;
    for (int r = 1; r <= radius; r++){
        for (i = pos_i - r; i <= pos_i + r; i++){
            for (j = pos_j - r; j <= pos_j +r; j++){
                if (!isValid(map_vector, {i, j})) continue;
                if (abs(i - pos_i) + abs(j - pos_j) != r || map_vector[i][j] == 1) continue;

                range = getThetaRange(pos, {i, j});
                if (range.second > 314 && range.first < 46 && map_vector[i][j] == 0){
                    degreeInterval = mergeInterval({0.0001, range.first}, degreeInterval);
                    degreeInterval = mergeInterval({range.second, 359.9999}, degreeInterval);
                    Vec[i][j] = 0;
                    continue;
                }
                if (checkInterval(range, degreeInterval)){
                    // if totally blocked by other obstacles, UAV was unaware of this obstacle
                    // have to consider another situation, that this [i][j] had been checked before
                    // and set as 0 which means unreachable, we can't make it invisible again in radar_map
                    // however, in image shown as to having used radar, it should be erased again
                    // that step is not necessary(for now)
                    // if (radar_map[i][j] == 0) radar_map[i][j] = 0;
                    // if (radar_map[i][j] == 1) radar_map[i][j] = 1;

                }else{
                    Vec[i][j] = 0;
                }
                degreeInterval = mergeInterval(range, degreeInterval);
            }
        }
    }

}

vector<pair<double, double>> Solution::mergeInterval(const pair<double, double> &newRange,
                                           vector<pair<double, double>> &intervals) {
    // https://leetcode-cn.com/problems/merge-intervals/solution/he-bing-qu-jian-by-leetcode-solution/

    intervals.emplace_back(newRange);

    sort(intervals.begin(), intervals.end());
    vector<pair<double, double>> merged;
    for (auto & interval : intervals) {
        double L = interval.first, R = interval.second;

        if (merged.empty() || merged.back().second < L) {
            merged.emplace_back(L, R);
        }
        else {
            merged.back().second = max(merged.back().second, R);
        }
    }
    return merged;
}

bool Solution::checkInterval(const pair<double, double> &newRange,
                   vector<pair<double, double>> &intervals) {
    bool value = false;
    if (intervals.empty()){
        return false;
    }

    sort(intervals.begin(), intervals.end());
    double left = newRange.first;
    double right = newRange.second;

    for (auto & interval : intervals){
        if (left >= interval.first && right <= interval.second){
            value = true;
            break;
        }
    }
    return value;
}

pair<double, double> Solution::getThetaRange(Pair point_0, Pair point_d){
    // point_0 center
    // point_d para

    double PI = 3.1415926;

    pair<double, double> range;

    int i_0 = point_0.first;
    int j_0 = point_0.second;
    int i_d = point_d.first;
    int j_d = point_d.second;

    // save theta
    double theta[4] = {0, 0, 0, 0};
    // range change from (-PI, PI) to (0, 2PI)
    // FIXME: special definition around -PI
    theta[0] = atan2(i_d + 0.5 - i_0, j_d + 0.5 - j_0) * 180 / PI + 180;
    theta[1] = atan2(i_d + 0.5 - i_0, j_d - 0.5 - j_0) * 180 / PI + 180;
    theta[2] = atan2(i_d - 0.5 - i_0, j_d + 0.5 - j_0) * 180 / PI + 180;
    theta[3] = atan2(i_d - 0.5 - i_0, j_d - 0.5 - j_0) * 180 / PI + 180;
    sort(theta, theta + 4);
    // if theta[3] >= 315, i_0 == i_d (for sure)

    // send to pair range
    range.second = theta[3];
    range.first = theta[0];

    if (theta[3] > 340 && theta[0] < 19){
        range.second = theta[2];
        range.first = theta[1];
    }

    return range;
}

void Solution::clearRadarMap() {
    cout << "map cleared" << endl;
    for (int i = 0; i < ROW; i++){
        for (int j = 0; j < COL; j++){
            radar_vector[i][j] = 1;
            check_vector[i][j] = 1;
            show_vector[i][j] = 1;
        }
    }
}

void Solution::launchRadar() {
    // clear all obstacles, forming a new map
    clearRadarMap();
    // initialize start point as StartPoint
    Pair pos = StartPoint;
    fetchLidarData(pos, radar_vector);

    int number = 0;
    cout << "into loop launch.." << endl;

    while(pos != EndPoint){
        // clear check_vector
        for (int i = 0; i < ROW; i++){
            for (int j = 0; j < COL; j++){
                check_vector[i][j] = 1;
            }
        }

        cout << "loop:" << number << endl;
        number = number + 1;

        printf("(%d, %d) pos\n", pos.first, pos.second);

        // store path in stack DynamicStack
        dynamicSearch(pos, radar_vector);

        // test if obstacle on path
        vector<Pair> path;
        while(!DynamicPath.empty()){
            Pair p = DynamicPath.top();
            DynamicPath.pop();
            path.emplace_back(p);
        }
        cout << "path data read in..." << endl;
        for (auto & i : path){
            cout << "data in path:" << i.first << ", " << i.second << endl;
        }

        PathVar message = ifPathObstacle(path);

        cout << get<0>(message) << endl;
        if (get<0>(message)){
            // update pos to last one
            pos = {get<1>(message), get<2>(message)};
            int num = get<3>(message);
            for (int i = 0; i <= num; i++){
                fetchLidarData(path[i], radar_vector);
                OverallPath.emplace_back(path[i]);

                cout << path[i].first << ", " << path[i].second << endl;
            }
        }if (!get<0>(message)){
            for (auto & i : path){
                OverallPath.push_back(i);
            }
            pos = EndPoint;
            cout << "end founded..." << endl;
        }
    }

}

void Solution::dynamicSearch(Pair &pos, Vec2int &m_grid) {
    // set begin time
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    Vec2int grid(ROW, vector<int>(COL));
    grid = getVectorBoundaryGrid(m_grid);

    // If the source is out of range
    if (!isValid(grid, StartPoint)) {
        printf("Source is invalid\n");
        return;
    }

    // If the destination is out of range
    if (!isValid(grid, EndPoint)) {
        printf("Destination is invalid\n");
        return;
    }

    // Either the source or the destination is blocked
    if (!isUnBlocked(grid, StartPoint)
        || !isUnBlocked(grid, EndPoint)) {
        printf("Source or the destination is blocked\n");
        return;
    }

    // If the destination cell is the same as source cell
    if (isDestination(StartPoint)) {
        printf("We are already at the destination\n");
        return;
    }

    // describe detail of each cell
    // using struct defined
    vector<vector <cell>> cellDetails(ROW, vector<cell>(COL));

    int i, j;
    // Initialising the parameters of the starting node
    i = pos.first, j = pos.second;
    cellDetails[i][j].cost_f = 0.0;
    cellDetails[i][j].cost_g = 0.0;
    cellDetails[i][j].cost_h = 0.0;
    cellDetails[i][j].nodeParent = {i, j};

    // Create a closed list and initialise it to false
    bool closedList[ROW][COL];
    memset(closedList, false, sizeof(closedList));

    /**
     * Create an open list having information as <f, <i, j>>
     * where f = g + h,
     * and i, j are the row and column index of that cell
     * Note that 0 <= i <= ROW-1 & 0 <= j <= COL-1
     * This open list is implemented as a set of tuple
     */
    priority_queue<Tuple, vector<Tuple>, greater<> > openList;
    // Type: Tuple
    // Container: vector
    // Functional: greater, least f will be on top of the queue

    openList.emplace(0.0, i, j);
    while(!openList.empty()){
        const Tuple& p = openList.top(); // first time, it will be start point
        // Add this vertex to the closed list
        i = get<1>(p); // second element of tuple
        j = get<2>(p); // third element of tuple

        // remove this vertex from open list and send it to close list
        openList.pop();
        closedList[i][j] = true;

        for (int add_x = -1; add_x <= 1; add_x++) {
            for (int add_y = -1; add_y <= 1; add_y++) {

                Pair neighbour(i + add_x, j + add_y);

                if (!isValid(grid, neighbour)){
                    return;
                }
                if (isDestination(neighbour)){
                    // set parent of destination cell
                    cellDetails[neighbour.first][neighbour.second].nodeParent = {i, j};
                    // print path
                    cout << "destination is found!" << endl;
                    dynamicTracePath(cellDetails, pos);
                    // set end time and show
                    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
                    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
                    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
                    // end
                    return;
                }
                else if (!closedList[neighbour.first][neighbour.second]
                         && isUnBlocked(grid, neighbour)){

                    // update weight values
                    double f_updated, g_updated, h_updated;
                    g_updated = cellDetails[i][j].cost_g + sqrt(pow(add_x, 2.0) + pow(add_y, 2.0));
                    h_updated = calculateHValue(neighbour, EndPoint);
                    f_updated = g_updated + h_updated;

                    // send less f_updated to open list
                    double temp_cost_f = cellDetails[neighbour.first][neighbour.second].cost_f;
                    if (f_updated < temp_cost_f || temp_cost_f == -1){
                        // -1 referring that this node doesn't belong to open list, shall be added in
                        // f_updated is less, shall be added in open list
                        // at first I was concerning about the StartPoint, then I just realized that
                        // StartPoint has been thrown into close list
                        openList.emplace(f_updated, neighbour.first, neighbour.second);

                        // update cost f, g, h and parent node of cellDetails
                        cellDetails[neighbour.first][neighbour.second].cost_f = f_updated;
                        cellDetails[neighbour.first][neighbour.second].cost_g = g_updated;
                        cellDetails[neighbour.first][neighbour.second].cost_h = h_updated;
                        // set parent of successors(not in Close list and is unblocked)
                        cellDetails[i + add_x][j + add_y].nodeParent = {i, j};
                        /**
                         * why updating parent node must exist here?
                         * if you actually put this update under definition of neighbour, some path might be confusing
                         * set updating here can assure that each node parent heading for gradient lifting
                         * namely f in decline
                         */
                    }

                }

            }

        }

    }


}

void Solution::dynamicTracePath(const vector<vector <cell>> &cellDetails, const Pair &pos) {
    cout << "trace started..." << endl;
    int i = EndPoint.first;
    int j = EndPoint.second;
    Pair temp_node;

    while(cellDetails[i][j].nodeParent != pos){
        DynamicPath.push({i, j});
        temp_node = cellDetails[i][j].nodeParent;
        // i = cellDetails[i][j].nodeParent.first is illegal!
        i = temp_node.first;
        j = temp_node.second;
    }
    DynamicPath.push({i, j});
    // push StartPoint into Stack
    DynamicPath.push(pos);
    cout << "trace finished" << endl;
}

PathVar Solution::ifPathObstacle(vector<Pair> &path){
    PathVar message;
    bool seg;

    // FIXME: check_map not best

    for (int n = 0; n < path.size(); n++){
        int i = path[n].first;
        int j = path[n].second;

        Pair pos = {i, j};
        fetchLidarData(pos, check_vector);

        for (auto & k : path){
            if (check_vector[k.first][k.second] == 0){
                seg = true;
                get<1>(message) = i;
                get<2>(message) = j;
                get<3>(message) = n;
                get<0>(message) = seg;
                printf("(%d, %d) is on obstacle\n", k.first, k.second);
                return message;
            }else{
                seg = false;
                get<0>(message) = seg;
            }
        }

    }

    return message;
}

void Solution::showPath() {
    for (int i = 0; i < ROW; i++){
        for (int j = 0; j < COL; j++){
            show_vector[i][j] = 1;
        }
    }

    Mat img;
    img = Mat(100, 100, CV_8UC3, Scalar::all(255));
    Vec2int temp_map (ROW, vector<int>(COL));
    for (auto & i : OverallPath){
        fetchLidarData(i, show_vector);
        temp_map = getVectorBoundaryGrid(show_vector);

        for (int x = 0; x < ROW; x++){
            for (int y = 0; y < COL; y++){
                if (show_vector[x][y] == 0) img.at<Vec3b>(x, y) = {0, 0, 0};
                if (show_vector[x][y] == 1 && temp_map[x][y] == 0)
                    img.at<Vec3b>(x, y) = {0, 128, 128};
            }
        }
        img.at<Vec3b>(i.first, i.second) = {0, 255, 0};
        img.at<Vec3b>(StartPoint.first, StartPoint.second) = {255, 0, 0};
        img.at<Vec3b>(EndPoint.first, EndPoint.second) = {0, 0, 255};
        namedWindow("dst", WINDOW_NORMAL);
        imshow("dst", img);

        waitKey(100);
    }

}


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
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623