hybrid_astar.hpp

Go to the documentation of this file.

#pragma once
#include <algorithm>
#include <cmath>
#include <iostream>
#include <unordered_map>
#include <unordered_set>

#define USE_FIBONACCI_HEAP
#include <boost/heap/d_ary_heap.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <boost/program_options.hpp>

#include "neighbor.hpp"
#include "planresult.hpp"

namespace libMultiRobotPlanning {
template <typename State, typename Action, typename Cost, typename Environment,
          typename StateHasher = std::hash<State>>
class HybridAStar {
 public:
  HybridAStar(Environment& environment) : m_env(environment) {}
  ~HybridAStar() {}

  bool search(const State& startState,
              PlanResult<State, Action, Cost>& solution, Cost initialCost = 0) {
    solution.states.clear();
    solution.actions.clear();
    solution.cost = 0;

    openSet_t openSet;
    std::unordered_map<uint64_t, heapHandle_t, std::hash<uint64_t>> stateToHeap;
    std::unordered_set<uint64_t, std::hash<uint64_t>> closedSet;
    std::unordered_map<State, std::tuple<State, Action, Cost, Cost>,
                       StateHasher>
        cameFrom;

    auto handle =
        openSet.push(Node(startState, Action(),
                          m_env.admissibleHeuristic(startState), initialCost));
    stateToHeap.insert(std::make_pair<>(m_env.calcIndex(startState), handle));
    (*handle).handle = handle;

    std::vector<Neighbor<State, Action, Cost>> neighbors;
    neighbors.reserve(10);

    while (!openSet.empty()) {
      Node current = openSet.top();
      m_env.onExpandNode(current.state, current.fScore, current.gScore);
      if (m_env.isSolution(current.state, current.gScore, cameFrom)) {
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(m_env.getGoal());
        solution.cost = std::get<3>(iter->second);
        solution.fmin =
            std::get<3>(iter->second) +
            m_env.admissibleHeuristic(iter->first);  // current.fScore;
        while (iter != cameFrom.end()) {
          // std::cout << " From " << std::get<0>(iter->second)
          //           << " to Node:" << iter->first
          //           << " with ACTION: " << std::get<1>(iter->second) << "
          //           cost "
          //           << std::get<2>(iter->second) << " g_score "
          //           << std::get<3>(iter->second) << std::endl;
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, initialCost));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());

        openSet.clear();
        return true;
      }

      openSet.pop();
      stateToHeap.erase(m_env.calcIndex(current.state));
      closedSet.insert(m_env.calcIndex(current.state));
      // traverse neighbors
      neighbors.clear();
      m_env.getNeighbors(current.state, current.action, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {
        if (closedSet.find(m_env.calcIndex(neighbor.state)) ==
            closedSet.end()) {  // not in closed set
          Cost tentative_gScore = current.gScore + neighbor.cost;
          auto iter = stateToHeap.find(m_env.calcIndex(neighbor.state));
          if (iter == stateToHeap.end()) {  // Discover a new node
            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);
            auto handle = openSet.push(Node(neighbor.state, neighbor.action,
                                            fScore, tentative_gScore));
            (*handle).handle = handle;
            stateToHeap.insert(

                std::make_pair<>(m_env.calcIndex(neighbor.state), handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;
          } else {
            auto handle = iter->second;
            // std::cout << "  this is an old node: " << tentative_gScore << ","
            // << (*handle).gScore << std::endl;
            // We found this node before with a better path
            if (tentative_gScore >= (*handle).gScore) {
              continue;
            }

            // update f and gScore
            Cost delta = (*handle).gScore - tentative_gScore;
            (*handle).gScore = tentative_gScore;
            (*handle).fScore -= delta;
            (*handle).state = neighbor.state;
            openSet.increase(handle);
            m_env.onDiscover(neighbor.state, (*handle).fScore,
                             (*handle).gScore);
          }

          // Best path for this node so far
          // TODO: this is not the best way to update "cameFrom", but otherwise
          // default c'tors of State and Action are required
          cameFrom.erase(neighbor.state);
          cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
        }
      }
    }
    openSet.clear();
    return false;
  }

 private:
  struct Node {
    Node(const State& state, Action action, Cost fScore, Cost gScore)
        : state(state), action(action), fScore(fScore), gScore(gScore) {}

    bool operator<(const Node& other) const {
      // Sort order
      // 1. lowest fScore
      // 2. highest gScore

      // Our heap is a maximum heap, so we invert the comperator function here
      if (fScore != other.fScore) {
        return fScore > other.fScore;
      } else {
        return gScore < other.gScore;
      }
    }

    friend std::ostream& operator<<(std::ostream& os, const Node& node) {
      os << "state: " << node.state << " fScore: " << node.fScore
         << " gScore: " << node.gScore;
      return os;
    }

    State state;
    Action action;
    Cost fScore;
    Cost gScore;

#ifdef USE_FIBONACCI_HEAP
    typename boost::heap::fibonacci_heap<Node>::handle_type handle;
#else
    typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                     boost::heap::mutable_<true>>::handle_type
        handle;
#endif
  };

#ifdef USE_FIBONACCI_HEAP
  typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  typedef typename openSet_t::handle_type heapHandle_t;
#else
  typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                           boost::heap::mutable_<true>>
      openSet_t;
  typedef typename openSet_t::handle_type heapHandle_t;
#endif

 private:
  Environment& m_env;
};

}  // namespace libMultiRobotPlanning