map_planner.h

Go to the documentation of this file.

#ifndef MPL_MAP_PLANNER_H
#define MPL_MAP_PLANNER_H

#include <mpl_planner/common/planner_base.h>
#include <mpl_planner/env/env_map.h>

namespace MPL {

template <int Dim>
using linkedHashMap =
    std::unordered_map<int, std::vector<std::pair<Waypoint<Dim>, int>>>;
template <int Dim>
class MapPlanner : public PlannerBase<Dim, Waypoint<Dim>> {
 public:
  MapPlanner(bool verbose);
  virtual void setMapUtil(const std::shared_ptr<MapUtil<Dim>>& map_util);
  void setSearchRegion(const vec_Vecf<Dim>& path, bool dense = false);
  void setSearchRadius(const Vecf<Dim>& radius);

  void setPotentialRadius(const Vecf<Dim>& radius);
  void setPotentialMapRange(const Vecf<Dim>& range);
  void setGradientWeight(decimal_t w);
  void setPotentialWeight(decimal_t w);

  vec_Vec3f getPotentialCloud(decimal_t h_max = 1.0);
  vec_Vec3f getGradientCloud(decimal_t h_max = 1.0, int i = 0);

  vec_Vecf<Dim> getSearchRegion() const;
  vec_Vecf<Dim> getLinkedNodes() const;
  void updateBlockedNodes(const vec_Veci<Dim>& pns);
  void updateClearedNodes(const vec_Veci<Dim>& pns);

  void updatePotentialMap(const Vecf<Dim>& pos);

  bool iterativePlan(const Waypoint<Dim>& start, const Waypoint<Dim>& goal,
                     const Trajectory<Dim>& raw_traj, int max_iter_num = 3);

 protected:
  void createMask();

  vec_E<Vecf<Dim>> calculateGradient(const Veci<Dim>& coord1,
                                     const Veci<Dim>& coord2);
  std::shared_ptr<MapUtil<Dim>> map_util_;

  mutable linkedHashMap<Dim> lhm_;

  int8_t H_MAX{100};

  Vecf<Dim> potential_radius_{Vecf<Dim>::Zero()};
  Vecf<Dim> potential_map_range_{Vecf<Dim>::Zero()};
  vec_E<std::pair<Veci<Dim>, int8_t>> potential_mask_;
  decimal_t pow_{1.0};
  vec_E<Vecf<Dim>> gradient_map_;

  Vecf<Dim> search_radius_{Vecf<Dim>::Zero()};
};

typedef MapPlanner<2> OccMapPlanner;

typedef MapPlanner<3> VoxelMapPlanner;
}  // namespace MPL

#endif