safety_zone.h

#ifndef MRS_LIB_SAFETYZONE_H
#define MRS_LIB_SAFETYZONE_H
 
#include <map>
#include <memory>
#include <mrs_lib/safety_zone/prism.h>
#include <vector>
#include <Eigen/Dense>
 
namespace mrs_lib
{
 
namespace safety_zone
{
 
class SafetyZone {
public:
  SafetyZone(std::unique_ptr<Prism> &&outer_border);
 
  // Cleaning the obstacles' memory is SafetyZone's responsibility
  SafetyZone(std::unique_ptr<Prism> &&outer_border, std::vector<std::unique_ptr<Prism>> &&obstacles);
 
  ~SafetyZone() = default;
 
  // Enable/Disable safety zone
  void enableSafetyZone(const bool enable);
 
  bool safetyZoneEnabled(void);
 
  // Controls, if 3d point lies within the prism
  bool isPointValid(const Point3d &point);
 
  // Convenient version of isPointIn(Point3d point)
  bool isPointValid(const double px, const double py, const double pz);
 
  // Controls, if 2d point lies within the polygon of prism (i.e. ignores max_z
  // and min_z of all prisms)
  bool isPointValid(const Point2d &point);
 
  // Convenient version of isPointIn(Point2d point)
  bool isPointValid(const double px, const double py);
 
  // The function divides the path into smaller segments (on average 20 segments
  // per meter) and validates each intermediate point to determine if the entire
  // path is valid.
  bool isPathValid(const Point3d &start, const Point3d &end);
 
  // 2d version of isPathValid(const Point3d start, const Point3d end), i.e.
  // ignores max_z and min_z of all prisms
  bool isPathValid(const Point2d &start, const Point2d &end);
 
  Prism getBorder() const;
 
  const std::map<int, std::unique_ptr<Prism>> &getObstacles() const;
 
  Prism getObstacle(const int index) const;
 
  int addObstacle(std::unique_ptr<Prism> obstacle);
 
  void deleteObstacle(const int id);
 
  // Helper method for text representation
  // void accept(Visitor &visitor);
 
private:
  std::unique_ptr<Prism> outer_border_;
  std::map<int, std::unique_ptr<Prism>> obstacles_;
  mutable std::mutex mutex_safety_zone_;
  int next_obstacle_id_ = 0;
  bool safety_zone_enabled_ = false;
 
  // Used to discretize between start and end for the path validation
  int _discretization_steps_ = 20;
}; // class SafetyZone
 
} // namespace safety_zone
} // namespace mrs_lib
 
#endif // MRS_LIB_SAFETYZONE_H