rrt.h

#ifndef RRT_H
#define RRT_H
 
#include <chrono>
 
#include "global_body_planner/planner_class.h"
 
#define TRAPPED 0
#define ADVANCED 1
#define REACHED 2
 
using namespace planning_utils;
 
 
class RRT {
 public:
  RRT();
 
  ~RRT();
 
  int attemptConnect(const State &s_existing, const State &s, double t_s,
                     StateActionResult &result,
                     const PlannerConfig &planner_config, int direction);
 
  int attemptConnect(const State &s_existing, const State &s,
                     StateActionResult &result,
                     const PlannerConfig &planner_config, int direction);
 
  virtual int extend(PlannerClass &T, const State &s,
                     const PlannerConfig &planner_config, int direction,
                     ros::Publisher &tree_pub);
 
  std::vector<int> pathFromStart(PlannerClass &T, int idx);
 
  void printPath(PlannerClass &T, std::vector<int> path);
 
  void getStatistics(double &plan_time, int &vertices_generated,
                     double &plan_length, double &path_duration,
                     double &dist_to_goal);
 
  bool newConfig(State s, State s_near, StateActionResult &result,
                 const PlannerConfig &planner_config, int direction,
                 ros::Publisher &tree_pub);
 
  std::vector<State> getStateSequence(PlannerClass &T, std::vector<int> path);
 
  std::vector<Action> getActionSequence(PlannerClass &T, std::vector<int> path);
 
 protected:
  const double prob_goal_thresh = 0.05;
 
  bool goal_found = false;
 
  std::chrono::duration<double> elapsed_total_;
 
  // Total time elapsed until the first solve
  std::chrono::duration<double> elapsed_to_first_;
 
  int success_ = 0;
 
  int num_vertices_;
 
  double path_length_;
 
  double path_duration_;
 
  double dist_to_goal_;
 
  visualization_msgs::MarkerArray tree_viz_msg_;
};
 
#endif