Lifelong Planning A* (LPA*)

Architectural Setup & State Representation

#include <queue>
#include <unordered_map>
#include <cmath>
#include <tuple>

struct State {
    int x, y;
    bool operator==(const State& other) const {
        return x == other.x && y == other.y;
    }
};

struct StateHash {
    size_t operator()(const State& s) const {
        return std::hash<int>()(s.x) ^ (std::hash<int>()(s.y) << 1);
    }
};

using Key = std::pair<double, double>;

struct NodeData {
    double g;
    double rhs;
};

struct PriorityQueueComparator {
    bool operator()(const std::pair<State, Key>& a, const std::pair<State, Key>& b) const {
        if (a.second.first != b.second.first) {
            return a.second.first > b.second.first; // Min-heap on primary key
        }
        return a.second.second > b.second.second; // Min-heap on secondary key
    }
};

Core LPA* Protocol Logic Implementation

class LPAStar {
private:
    State start, goal;
    std::unordered_map<State, NodeData, StateHash> node_map;
    std::priority_queue<std::pair<State, Key>, std::vector<std::pair<State, Key>>, PriorityQueueComparator> open_set;

    double heuristic(const State& a, const State& b) {
        return std::abs(a.x - b.x) + std::abs(a.y - b.y); // Manhattan distance
    }

    Key calculateKey(const State& s) {
        double g_rhs_min = std::min(getG(s), getRhs(s));
        return {g_rhs_min + heuristic(s, goal), g_rhs_min};
    }

    double getG(const State& s) {
        auto it = node_map.find(s);
        if (it == node_map.end()) return INFINITY;
        return it->second.g;
    }

    double getRhs(const State& s) {
        if (s.x == start.x && s.y == start.y) return 0.0;
        auto it = node_map.find(s);
        if (it == node_map.end()) return INFINITY;
        return it->second.rhs;
    }

    std::vector<State> getPredecessors(const State& s) {
        // Return valid 4-connected or 8-connected grid neighbors
        return {{s.x+1, s.y}, {s.x-1, s.y}, {s.x, s.y+1}, {s.x, s.y-1}};
    }

    std::vector<State> getSuccessors(const State& s) {
        return getPredecessors(s);
    }

    double cost(const State& u, const State& v) {
        // Return edge cost, e.g., 1.0 for free space, INFINITY for obstacles
        return 1.0;
    }

public:
    void updateVertex(const State& u) {
        if (!(u == start)) {
            double min_rhs = INFINITY;
            for (const auto& pred : getPredecessors(u)) {
                min_rhs = std::min(min_rhs, getG(pred) + cost(pred, u));
            }
            node_map[u].rhs = min_rhs;
        }
        // Remove u from open if present (simplified logic: re-evaluate during pop)
        if (getG(u) != getRhs(u)) {
            // Push to open set with updated key
            // Production code should handle duplicate queue entries or lazy deletion
        }
    }
};

Verification, Profiling & Unit Testing

#include <cassert>

void runUnitTests() {
    State start{0, 0};
    State goal{5, 5};
    // Initialize graph, assert initial shortest path optimality
    assert(start.x == 0);
}

int main() {
    runUnitTests();
    return 0;
}

Production Hardening & Lazy Deletion Optimization

void cleanTopOpenSet() {
    while (!open_set.empty()) {
        State u = open_set.top().first;
        Key k_old = open_set.top().second;
        Key k_current = calculateKey(u);
        if (k_old.first < k_current.first || (k_old.first == k_current.first && k_old.second < k_current.second)) {
            // Stale entry, pop and discard
            open_set.pop();
        } else if (getG(u) == getRhs(u)) {
            // Locally consistent, discard
            open_set.pop();
        } else {
            break; // Valid top element found
        }
    }
}