Untitled

from queue import PriorityQueue

class HeuristicPlanner(PDDLPlanner):
    def __init__(self, heuristic=MaxHeuristic(), verbose=False, collect_stats=False):
        super().__init__(verbose, collect_stats)
        self.h = heuristic

    def solve(self, actions, state, goals):
        frontier = PriorityQueue()
        closedSet = set()
        initialCost = self.h.h(actions, state, goals)
        frontier.put((initialCost, id(state), state, []))

        while not frontier.empty():
            _, _, currentState, currentPlan = frontier.get()

            if self._goalsAchieved(currentState, goals):
                return currentPlan

            if currentState in closedSet:
                continue
            closedSet.add(currentState)

            for action in actions:
                if self._isApplicable(currentState, action):
                    nextState = self._applyAction(currentState, action)

                    if nextState in closedSet:
                        continue

                    g = len(currentPlan) + 1
                    h = self.h.h(actions, nextState, goals)
                    if h == float("inf"):
                        continue
                    totalCost = g + h

                    frontier.put((totalCost, id(nextState), nextState, currentPlan + [action]))

        return None

    def _goalsAchieved(self, state, goals):
        positiveGoals, negativeGoals = goals
        return positiveGoals.issubset(state) and not any(goal in state for goal in negativeGoals)

    def _isApplicable(self, state, action):
        return action.positive_preconditions.issubset(state)

    def _applyAction(self, state, action):
        newState = set(state)
        newState.update(action.add_effects)
        newState.difference_update(action.del_effects)
        return frozenset(newState)