Environment.h

Go to the documentation of this file.

#pragma once
 
#include <ompl/base/ScopedState.h>
 
#include "base/Primitives.h"
#include "utils/Stopwatch.hpp"
 
class Environment {
 public:
  Environment();
  virtual ~Environment() = default;
 
  void setStart(const Point &point);
  const Point &start() const { return _start; }
 
  void setGoal(const Point &point);
  const Point &goal() const { return _goal; }
 
  virtual bool collides(double x, double y) { return true; }
  virtual bool collides(const Polygon &polygon) { return true; }
  bool collides(const Point &p) { return collides(p.x, p.y); }
  bool collides(const ompl::geometric::PathGeometric &trajectory);
  bool collides(const ob::State *state) {
    const auto *s = state->as<State>();
    return collides(s->getX(), s->getY());
  }
 
  bool checkValidity(const ob::State *state);
 
  inline const ob::RealVectorBounds &bounds() const { return _bounds; }
  inline double width() const { return _bounds.high.at(0) - _bounds.low.at(0); }
  inline double height() const {
    return _bounds.high.at(1) - _bounds.low.at(1);
  }
 
  virtual double distance(double x, double y) { return -1; }
 
  double distance(const ob::State *state) {
    const auto *s = state->as<State>();
    return distance(s->getX(), s->getY());
  }
 
  double bilinearDistance(double x, double y, double cellSize = 1);
  double bilinearDistance(const Point &point, double cellSize = 1) {
    return bilinearDistance(point.x, point.y, cellSize);
  }
  double bilinearDistance(const ob::State *state, double cellSize = 1) {
    const auto *s = state->as<State>();
    return bilinearDistance(s->getX(), s->getY());
  }
 
  bool distanceGradient(double x, double y, double &dx, double &dy,
                        double p = 0.1, double cellSize = 1);
 
  virtual std::string name() const { return "Base map"; }
 
  void estimateStartGoalOrientations();
 
  bool thetasDefined() const { return _thetas_defined; }
  void setThetas(double start, double goal);
 
  ompl::base::State *startState() const { return _start.toState(_start_theta); }
  ompl::base::State *goalState() const { return _goal.toState(_goal_theta); }
 
  ompl::base::ScopedState<ob::SE2StateSpace> startScopedState() const;
  ompl::base::ScopedState<ob::SE2StateSpace> goalScopedState() const;
 
  ob::RealVectorBounds getBounds() const {return _bounds;} 
 
  double startTheta() const { return _start_theta; }
  double goalTheta() const { return _goal_theta; }
 
  virtual void to_json(nlohmann::json &j) {
    j["type"] = "base";
    j["width"] = width();
    j["height"] = height();
    j["start"] = start();
    j["goal"] = goal();
    j["name"] = name();
  }
 
  virtual double unit() const { return 1; }
 
  void resetCollisionTimer() { _collision_timer.reset(); }
  double elapsedCollisionTime() const { return _collision_timer.elapsed(); }
 
 protected:
  Point _start;
  Point _goal;
  double _start_theta{0};
  double _goal_theta{0};
  bool _thetas_defined{false};
 
  ob::RealVectorBounds _bounds{2};
 
  Stopwatch _collision_timer;
};