gbpl.h

#ifndef GBPL_H
#define GBPL_H
 
#include "global_body_planner/rrt.h"
 
#define TRAPPED 0
#define ADVANCED 1
#define REACHED 2
 
using namespace planning_utils;
 
 
class GBPL : public RRT {
 public:
  GBPL();
 
  int connect(PlannerClass &T, State s, const PlannerConfig &planner_config,
              int direction, ros::Publisher &tree_pub);
 
  std::vector<Action> getActionSequenceReverse(PlannerClass &T,
                                               std::vector<int> path);
 
  void postProcessPath(std::vector<State> &state_sequence,
                       std::vector<Action> &action_sequence,
                       const PlannerConfig &planner_config);
 
  void extractPath(PlannerClass &Ta, PlannerClass &Tb,
                   std::vector<State> &state_sequence,
                   std::vector<Action> &action_sequence,
                   const PlannerConfig &planner_config);
 
  void extractClosestPath(PlannerClass &Ta, const State &s_goal,
                          std::vector<State> &state_sequence,
                          std::vector<Action> &action_sequence,
                          const PlannerConfig &planner_config);
 
  int findPlan(const PlannerConfig &planner_config, State s_start, State s_goal,
               std::vector<State> &state_sequence,
               std::vector<Action> &action_sequence, ros::Publisher &tree_pub);
 
 protected:
  double anytime_horizon;
 
  const double planning_rate_estimate =
      16.0;  // m/s (meters planned/computation time)
 
  double anytime_horizon_init;
 
  const double horizon_expansion_factor = 1.2;
};
 
#endif  // GBPL_H