Program Listing for File lpa_star.hpp

↰ Return to documentation for file (include/path_planning/lpa_star.hpp)

#ifndef LPA_STAR_H
#define LPA_STAR_H

#include <iostream>
#include <tuple>
#include <unordered_map>
#include <unordered_set>

#include "path_planning/planner.hpp"
#include "utils/utils.hpp"

class LPAStar : public Planner {
 public:
  explicit LPAStar(std::vector<std::vector<int>> grid)
      : Planner(std::move(grid)) {}

  void SetDynamicObstacles(const bool create_random_obstacles = false,
                           const std::unordered_map<int, std::vector<Node>>&
                               time_discovered_obstacles = {}) override;

  std::tuple<bool, std::vector<Node>> Plan(const Node& start,
                                           const Node& goal) override;

 private:
  Key CalculateKey(const Node& s) const;

  std::vector<Node> GetPred(const Node& u) const;

  std::vector<Node> GetSucc(const Node& u) const;

  std::vector<Node> GetNeighbours(const Node& u) const;

  double C(const Node& s1, const Node& s2) const;

  void Initialize();

  void UpdateVertex(const Node& u);

  bool CompareKey(const Key& p1, const Key& p2) const;

  void ComputeShortestPath();

  std::vector<Node> DetectChanges();

  bool IsObstacle(const Node& n) const;

  static double H(const Node& n1, const Node& n2);

  std::vector<std::vector<double>> CreateGrid();

  void ClearPathDisplay(const std::vector<Node>& path);
  std::vector<Node> GetNewPath();
  void UpdatePathDisplay(const std::vector<Node>& path);

  std::vector<std::vector<double>> rhs_;
  std::vector<std::vector<double>> g_;
  std::unordered_map<int, std::vector<Node>> time_discovered_obstacles_ = {};
  std::vector<Node> motions_;
  LazyPQ U_;
  Node start_, goal_;
  size_t time_step_ = 0;
  size_t max_time_step_ = 10;
  bool create_random_obstacles_ = false;
};

#endif  // LPA_STAR_H