distance_map_planner.h

Go to the documentation of this file.

#ifndef DMPLANNER_H
#define DMPLANNER_H

#include "graph_search.h"
#include <jps_collision/map_util.h>
#include <jps_basis/data_type.h>

class GraphSearch;
template <int Dim> class DMPlanner {
public:
  DMPlanner(bool verbose = false);

  std::vector<bool> setPath(const vec_Vecf<Dim> &path, const Vecf<Dim>& radius,
                            bool dense);

  void setSearchRadius(const Vecf<Dim>& r);
  void setPotentialRadius(const Vecf<Dim>& r);
  void setPotentialMapRange(const Vecf<Dim>& r);
  void setEps(double eps);
  void setCweight(double c);
  void setPow(int pow);

  int status();
  vec_Vecf<Dim> getPath();
  vec_Vecf<Dim> getRawPath();
  vec_Vecf<Dim> getPriorPath();
  vec_Vecf<Dim> getOpenSet() const;
  vec_Vecf<Dim> getCloseSet() const;
  vec_Vecf<Dim> getAllSet() const;
  vec_Vec3f getCloud(double h_max = 1);
  vec_Vecf<Dim> getSearchRegion();
  std::shared_ptr<JPS::MapUtil<Dim>> getMapUtil();

  void setMap(const std::shared_ptr<JPS::MapUtil<Dim>> &map_util,
              const Vecf<Dim>& pos);

  bool computePath(const Vecf<Dim>& start, const Vecf<Dim>& goal, const vec_Vecf<Dim>& path);
protected:
  vec_E<std::pair<Veci<Dim>, int8_t>> createMask(int pow);
  bool plan(const Vecf<Dim> &start, const Vecf<Dim> &goal,
            decimal_t eps = 1, decimal_t cweight = 0.1);
  vec_Vecf<Dim> removeLinePts(const vec_Vecf<Dim> &path);
  vec_Vecf<Dim> removeCornerPts(const vec_Vecf<Dim> &path);
  bool checkAvailability(const Veci<Dim> &pn);

  std::shared_ptr<JPS::MapUtil<Dim>> map_util_;
  std::shared_ptr<DMP::GraphSearch> graph_search_;
  std::vector<bool> search_region_;
  std::vector<int8_t> cmap_;

  bool planner_verbose_;
  double path_cost_;
  vec_Vecf<Dim> prior_path_;
  vec_Vecf<Dim> raw_path_;
  vec_Vecf<Dim> path_;
  int status_ = 0;
  int8_t H_MAX{100};
  double eps_{0.0};
  double cweight_{0.1};
  Vecf<Dim> potential_radius_{Vecf<Dim>::Zero()};
  Vecf<Dim> search_radius_{Vecf<Dim>::Zero()};
  Vecf<Dim> potential_map_range_{Vecf<Dim>::Zero()};
  int pow_{1};

};

typedef DMPlanner<2> DMPlanner2D;

typedef DMPlanner<3> DMPlanner3D;


template <int Dim> class IterativeDMPlanner : public DMPlanner<Dim> {
 public:
  IterativeDMPlanner(bool verbose = false);

  bool iterativeComputePath(const Vecf<Dim> &start, const Vecf<Dim> &goal,
                            const vec_Vecf<Dim> &path, int max_iteration);
};

typedef IterativeDMPlanner<2> IterativeDMPlanner2D;

typedef IterativeDMPlanner<3> IterativeDMPlanner3D;



#endif