PathPlanning-Astar/src/astarquadtree.cpp at main · CPPavithra/PathPlanning-Astar · GitHub

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
#include <iostream>
#include <vector>
#include <cmath>
#include <unordered_map>
#include <queue>
#include <algorithm>
#include <fstream>
#include <sstream>
#include <librealsense2/rs.hpp>
#include <vector>
#include <rerun.hpp>
#include <Eigen/Dense>
#include <thread>
#include <mutex>
#include <chrono>
#include <unordered_map>
#include <functional>
#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <cstdlib>
#include <rerun/demo_utils.hpp>
#include <unordered_set>
#include <set>
#include <limits>
#include "pathplanning.h"

using namespace std;

namespace planning {
double heuristic(float x1, float y1, float x2, float y2) {
    return hypot(x2 - x1, y2 - y1);
}

vector<Node> reconstruct_path(shared_ptr<Node> current) {
    vector<Node> path;
    while (current != nullptr) {
        path.push_back(*current);
        current = current->parent;
    }
    reverse(path.begin(), path.end());
    return path;
}

int roundToGrid(float coord, float resolution) {
    return static_cast<int>(round(coord / resolution));
}

int getCostAtPoint(mapping::Point p, quadtree::QuadtreeNode* low, quadtree::QuadtreeNode* mid, quadtree::QuadtreeNode* high) {
    int cost = 1; // Default cost for free space
    if (low->inBounds(p)) cost = max(cost, low->getCostAtPoint(p));
    if (mid->inBounds(p)) cost = max(cost, mid->getCostAtPoint(p));
    if (high->inBounds(p)) cost = max(cost, high->getCostAtPoint(p));
    return cost;
}


vector<Node> astarquad(quadtree::QuadtreeNode* lowQuadtree, quadtree::QuadtreeNode* midQuadtree, quadtree::QuadtreeNode* highQuadtree,
                       Node start, Node goal, float resolution) {
    auto cmp = [](const shared_ptr<Node>& a, const shared_ptr<Node>& b) {
        return a->f_cost > b->f_cost;
    };
    priority_queue<shared_ptr<Node>, vector<shared_ptr<Node>>, decltype(cmp)> openSet(cmp);

    unordered_map<pair<int, int>, double, NodeHasher> gScore;
    unordered_map<pair<int, int>, shared_ptr<Node>, NodeHasher> cameFrom;
    unordered_map<pair<int, int>, bool, NodeHasher> closedSet;

    pair<int, int> startKey = {planning::roundToGrid(start.x, resolution), planning::roundToGrid(start.y, resolution)};
    shared_ptr<Node> startNode = make_shared<Node>(start.x, start.y, 0, planning::heuristic(start.x, start.y, goal.x, goal.y));
    openSet.push(startNode);
    gScore[startKey] = 0.0;

    const float dx[8] = {resolution, -resolution, 0, 0, resolution, -resolution, resolution, -resolution};
    const float dy[8] = {0, 0, resolution, -resolution, resolution, -resolution, -resolution, resolution};

    while (!openSet.empty()) {
        shared_ptr<Node> current = openSet.top();
        openSet.pop();

        pair<int, int> currentKey = {planning::roundToGrid(current->x, resolution), planning::roundToGrid(current->y, resolution)};
        if (closedSet[currentKey]) continue;
        closedSet[currentKey] = true;

        if (planning::heuristic(current->x, current->y, goal.x, goal.y) < resolution) {
            return reconstruct_path(current);
        }


        for (int dir = 0; dir < 8; dir++) {
            float newX = current->x + dx[dir];
            float newY = current->y + dy[dir];
            pair<int, int> neighborKey = {planning::roundToGrid(newX, resolution), planning::roundToGrid(newY, resolution)};
            if (closedSet[neighborKey]) continue;

            mapping::Point neighborP = {newX, newY};
            int cost = planning::getCostAtPoint(neighborP, lowQuadtree, midQuadtree, highQuadtree);
            if (cost >= 10000) continue;

            double tentative_g = current->g_cost + resolution * cost;
            if (!gScore.count(neighborKey) || tentative_g < gScore[neighborKey]) {
                gScore[neighborKey] = tentative_g;
                auto neighbor = make_shared<Node>(newX, newY, tentative_g, planning::heuristic(newX, newY, goal.x, goal.y), current);
                openSet.push(neighbor);
                cameFrom[neighborKey] = neighbor;
            }
        }
    }

    return {};
}
}