navigator.hpp

Go to the documentation of this file.

#ifndef REDNAV_LIBS_NAVIGATOR_HPP
#define REDNAV_LIBS_NAVIGATOR_HPP

#include <rednav/sensorModel.hpp>
#include <rednav/pathTool.hpp>
#include <lgl/NavGraph.hpp>
#include <lgl/GeoData.hpp>
#include <lgl/Location.hpp>
#include <iostream>
#include <string>


namespace jafar {
namespace rednav {

  class Navigator {
    private:
      int xsize;
      int ysize;
      int zsize;
      double xMapOrigin;
      double yMapOrigin;
      double zMapOrigin;
      double pixelScale;
      int startx;
      int starty;
      int startz;
      double startutmx;
      double startutmy;
      double startutmz;
      int goalx;
      int goaly;
      int goalz;
      double goalutmx;
      double goalutmy;
      double goalutmz;

      jafar::lgl::GeoData *navMap;

      jafar::lgl::Quadtree<double, jafar::lgl::GeoData> *navQuadtree;

      jafar::lgl::NavGraph *navGraph;

      PathTool *multiTool;

      bool isDrawingNSU;

    public:

      Navigator();

      Navigator(const std::string& filepath);

      Navigator(double* bufferedData, int xsize_, int ysize_, int zsize_, double xMapOrigin_, double yMapOrigin_, double zMapOrigin_, double pixelScale_);

      ~Navigator();

      SensorModel *sensor;

      bool isUp();

      bool savePath(const std::string& outfile,  jafar::lgl::NavPath& savingpath);

      bool saveData(const std::string& outfile, const std::string& toutfile, const std::string& quadTreeFile);

      bool saveGeodata(const std::string& outfile);

      bool fastFindPath(const jafar::lgl::NavLocation& locstart, const jafar::lgl::NavLocation& locend, jafar::lgl::NavPath& navpath);

      bool findPath(const jafar::lgl::NavLocation& locstart, const jafar::lgl::NavLocation& locend, jafar::lgl::NavPath& navpath);

      bool findPath(const jafar::lgl::NavLocation& locstart, const jafar::lgl::NavLocation& locend, jafar::lgl::NavPath& navpath, double& pathCost);

      bool findPathOnUtility(const jafar::lgl::NavLocation& locstart, const jafar::lgl::NavLocation& locend, jafar::lgl::NavPath& navpath, double authDist=0.0);

      bool updateMap(double* bufferedData, int xsize_, int ysize_);

      bool updateMap(jafar::lgl::GeoData *inputGData);

      //-------------------------------------- GET
      void printInfo();

      jafar::lgl::GeoData* getNavMap() { return navMap; };

      jafar::lgl::NavGraph::NavGraphContainer* getGraph();

      jafar::lgl::Decomp* getDecomposition();

      std::list<jafar::lgl::NavGraph::frontierPoint> getNavGraphFrontiers();

      bool getIsDrawingNSU();

      //-------------------------------------- SET 
      bool setMapOrigin(double xMapOrigin_, double yMapOrigin_, double zMapOrigin_, double pixelScale_);

      void setMapSize(int  xsize_, int ysize_, int zsize_);

      void setGoal(const jafar::lgl::NavLocation& A);
      void setGoal(int x, int y, int z);
      void setGoal(double x, double y, double z);

      void setStart(const jafar::lgl::NavLocation& A);
      void setStart(int x, int y, int z);
      void setStart(double x, double y, double z);

      void setPathCropers(double pce, double pcp);

      void setMaxWayPts(int maxwpts);

      void setSigmaFactor(double sig_);

      void setProbabilityLag(double probaLag_);

      void setPathToolPathMaxDistance(double pmd);

      void setPathToolSubSampler(double setsbs);

      void setPathToolInterpolationDist(double);

      void setPathToolInterpolationIndexStart(int);

      void setPathToolInterpolationIndexStop(int);

      void setIsDrawingNSU(bool B);

      void setPathMaxStepDistance(double pmsd);

      //-------------------------------------- Utils

      bool findUtilityStartFromFrontiers(jafar::lgl::NavLocation& autoStart);

      bool doCalculateNavigationUtility();

      bool cropPath(double vent, double vproba, jafar::lgl::NavPath& navpath);

      bool applyTool(jafar::lgl::NavPath& srcPath, jafar::lgl::NavPath& outPath, PATH_TRANSFORMATION);

  };
}} // end of namespaces

#endif //REDNAV_LIBS_NAVIGATOR_HPP