Program Listing for File rrt_star.hpp

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

#ifndef RRT_STAR_H
#define RRT_STAR_H

#include <limits>
#include <tuple>
#include <unordered_map>
#include <vector>

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

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

  void SetParams(const int threshold = 2, const int max_iter_x_factor = 20);

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

 private:
  std::tuple<bool, Node> FindNearestPoint(Node& new_node);

  bool IsAnyObstacleInPath(const Node& n_1, const Node& n_2) const;

  Node GenerateRandomNode() const;

  void Rewire(const Node& new_node);

  bool CheckGoalVisible(const Node& new_node);

  void CreateObstacleList();

  std::vector<Node> CreatePath();

 private:
  Node start_, goal_;
  std::unordered_set<Node, NodeIdAsHash, compare_coordinates>
      point_list_;  // TODO: set up in cstor
  std::unordered_map<Node, std::vector<Node>> near_nodes_;
  std::vector<Node> obstacle_list_;
  double threshold_ = 1.5;       // TODO: set up in cstor
  int max_iter_x_factor_ = 500;  // TODO: set up in cstor
};

#endif  // RRT_STAR_H