AnyMotionPlanner.h

#ifndef ANY_MOTION_PLANNER_H
#define ANY_MOTION_PLANNER_H

#include <KrisLibrary/Logger.h>
#include "MotionPlanner.h"

class TiXmlElement;

//forward declarations
class HaltingCondition;
class MotionPlanningProblem;
class MotionPlannerInterface;
class MotionPlannerFactory;
class PropertyMap;

class HaltingCondition
{
 public:
  HaltingCondition();
  bool LoadJSON(const std::string& str);
  std::string SaveJSON() const;

  bool foundSolution;
  int maxIters;
  Real timeLimit;
  Real costThreshold;
  Real costImprovementPeriod;
  Real costImprovementThreshold;
};

class MotionPlannerInterface
{
 public:
  typedef Graph::UndirectedGraph<Config,std::shared_ptr<EdgePlanner> > Roadmap;

  MotionPlannerInterface() {}
  virtual ~MotionPlannerInterface() {}
  virtual std::string Plan(MilestonePath& path,const HaltingCondition& cond);
  virtual int PlanMore()=0;
  virtual void PlanMore(int numIters) { for(int i=0;i<numIters;i++) PlanMore(); }
  virtual int NumIterations() const=0;
  virtual int NumMilestones() const=0;
  virtual int NumComponents() const=0;
  virtual bool CanAddMilestone() const { return false; }
  virtual int AddMilestone(const Config& q)=0;
  virtual void GetMilestone(int,Config& q)=0;
  virtual void ConnectHint(int m) { }
  virtual bool ConnectHint(int ma,int mb) { return false; }
  virtual bool IsConnected(int ma,int mb) const=0;
  virtual bool IsPointToPoint() const { return true; }
  virtual bool IsOptimizing() const { return false; }
  virtual bool CanUseObjective() const { return false; }
  virtual void SetObjective(std::shared_ptr<ObjectiveFunctionalBase> obj) {}
  virtual bool IsLazy() const { return false; }
  virtual bool IsLazyConnected(int ma,int mb) const { return IsConnected(ma,mb); }
  virtual bool CheckPath(int ma,int mb) { return false; }
  virtual int GetClosestMilestone(const Config& q);
  virtual void GetPath(int ma,int mb,MilestonePath& path)=0;
  virtual Real GetOptimalPath(int ma,const std::vector<int>& mb,MilestonePath& path) { return Inf; }
  virtual bool IsSolved() { return IsConnected(0,1); }
  virtual void GetSolution(MilestonePath& path) { return GetPath(0,1,path); }
  virtual void GetRoadmap(Roadmap& roadmap) const {}
  virtual void GetStats(PropertyMap& stats) const;
};


class MotionPlanningProblem
{
 public:
  MotionPlanningProblem();
  MotionPlanningProblem(CSpace* space,const Config& a,const Config& b);
  MotionPlanningProblem(CSpace* space,const Config& a,CSet* goalSet);
  MotionPlanningProblem(CSpace* space,CSet* startSet,CSet* goalSet);
  MotionPlanningProblem(CSpace* space,const Config& a,std::shared_ptr<ObjectiveFunctionalBase> objective);

  CSpace* space;
  Config qstart,qgoal;
  CSet *startSet, *goalSet;
  std::shared_ptr<ObjectiveFunctionalBase> objective;
};

class MotionPlannerFactory
{
 public:
  MotionPlannerFactory();
  virtual ~MotionPlannerFactory() {}
  virtual MotionPlannerInterface* Create(const MotionPlanningProblem& problem);
  virtual MotionPlannerInterface* Create(CSpace* space);
  virtual MotionPlannerInterface* Create(CSpace* space,const Config& a,const Config& b);
  virtual MotionPlannerInterface* Create(CSpace* space,const Config& a,CSet* goalSet);
  virtual MotionPlannerInterface* CreateRaw(CSpace* space);
  virtual MotionPlannerInterface* ApplyModifiers(MotionPlannerInterface*,const MotionPlanningProblem& problem);
  bool Load(TiXmlElement* e);
  bool Save(TiXmlElement* e);
  bool LoadJSON(const std::string& str);
  std::string SaveJSON() const;

  std::string type;
  int knn;                 
  Real connectionThreshold;
  Real suboptimalityFactor;
  bool ignoreConnectedComponents; //for PRM (default false)
  Real perturbationRadius; 
  int perturbationIters;   
  bool bidirectional;      
  bool useGrid;            
  Real gridResolution;     
  int randomizeFrequency;  
  std::string pointLocation;    
  bool storeEdges;         
  bool shortcut;           
  bool restart;            
  std::string restartTermCond;  
};



#endif