MPLibrary.h

Go to the documentation of this file.

#ifndef PMPL_MP_LIBRARY_H_
#define PMPL_MP_LIBRARY_H_
 
#include "MPLibrary/MPSolution.h"
 
#include "MPLibrary/MPBaseObject.h"
 
#include "MPProblem/MPProblem.h"
#include "MPProblem/MPTask.h"
#include "MPProblem/GroupTask.h"
#include "MPProblem/Robot/Robot.h"
#include "MPProblem/RobotGroup/RobotGroup.h"
 
#include "Utilities/MetricUtils.h"
#include "Utilities/MPUtils.h"
#include "Utilities/XMLNode.h"
#include "Utilities/MethodSet.h"
 
#include "Traits/Methods.h"
 
#include "MPLibrary/Connectors/ConnectorMethod.h"
#include "MPLibrary/DistanceMetrics/DistanceMetricMethod.h"
#include "MPLibrary/Extenders/ExtenderMethod.h"
#include "MPLibrary/LocalPlanners/LocalPlannerMethod.h"
#include "MPLibrary/MapEvaluators/MapEvaluatorMethod.h"
#include "MPLibrary/Metrics/MetricMethod.h"
#include "MPLibrary/MPStrategies/MPStrategyMethod.h"
#include "MPLibrary/MPTools/MPTools.h"
#include "MPLibrary/NeighborhoodFinders/NeighborhoodFinderMethod.h"
#include "MPLibrary/PathModifiers/PathModifierMethod.h"
#include "MPLibrary/EdgeValidityCheckers/EdgeValidityCheckerMethod.h"
#include "MPLibrary/Samplers/SamplerMethod.h"
#include "MPLibrary/ValidityCheckers/CollisionDetectionValidity.h"
#include "MPLibrary/ValidityCheckers/ValidityCheckerMethod.h"
#include "MPLibrary/GoalTracker.h"
 
 
#ifdef _PARALLEL
class MPLibrary : public stapl::p_object
#else
class MPLibrary
#endif
{
 
  public:
 
 
    typedef MPSolutionType MPSolution;
    typedef GenericStateGraph<Cfg, DefaultWeight<Cfg>> RoadmapType;
    typedef GroupRoadmap<GroupCfg<RoadmapType>, GroupLocalPlan<RoadmapType>> GroupRoadmapType;
    typedef size_t VID;
    typedef MPToolsType MPTools;
    typedef GoalTrackerType GoalTracker;
 
 
    struct Solver {
      std::string label;         
      long seed;                 
      std::string baseFilename;  
      bool vizmoDebug;           
    };
 
 
    typedef MethodSet<DistanceMetricMethod> DistanceMetricSet;
    typedef MethodSet<ValidityCheckerMethod>
                                                            ValidityCheckerSet;
    typedef MethodSet<NeighborhoodFinderMethod>
                                                            NeighborhoodFinderSet;
    typedef MethodSet<SamplerMethod>        SamplerSet;
    typedef MethodSet<LocalPlannerMethod>   LocalPlannerSet;
    typedef MethodSet<ExtenderMethod>       ExtenderSet;
    typedef MethodSet<PathModifierMethod>   PathModifierSet;
    typedef MethodSet<EdgeValidityCheckerMethod>
                                                                EdgeValidityCheckerSet;
    typedef MethodSet<ConnectorMethod>      ConnectorSet;
    typedef MethodSet<MetricMethod>         MetricSet;
    typedef MethodSet<MapEvaluatorMethod>   MapEvaluatorSet;
    typedef MethodSet<MPStrategyMethod>     MPStrategySet;
 
 
    typedef typename ValidityCheckerSet::MethodPointer ValidityCheckerPointer;
    typedef typename DistanceMetricSet::MethodPointer  DistanceMetricPointer;
    typedef typename NeighborhoodFinderSet::MethodPointer
                                                       NeighborhoodFinderPointer;
    typedef typename SamplerSet::MethodPointer         SamplerPointer;
    typedef typename LocalPlannerSet::MethodPointer    LocalPlannerPointer;
    typedef typename ExtenderSet::MethodPointer        ExtenderPointer;
    typedef typename PathModifierSet::MethodPointer    PathModifierPointer;
    typedef typename EdgeValidityCheckerSet::MethodPointer
                                                       EdgeValidityCheckerPointer;
    typedef typename ConnectorSet::MethodPointer       ConnectorPointer;
    typedef typename MetricSet::MethodPointer          MetricPointer;
    typedef typename MapEvaluatorSet::MethodPointer    MapEvaluatorPointer;
    typedef typename MPStrategySet::MethodPointer      MPStrategyPointer;
 
 
    MPLibrary();
 
    MPLibrary(const std::string& _filename);
 
    MPLibrary(XMLNode& planningLibraryNode);
 
    virtual ~MPLibrary();
 
 
    void ReadXMLFile(const std::string& _filename);
 
    void ProcessXML(XMLNode &node);
 
 
    DistanceMetricPointer GetDistanceMetric(const std::string& _l) {
      return m_distanceMetrics->GetMethod(_l);
    }
    void AddDistanceMetric(DistanceMetricPointer _dm, const std::string& _l) {
      m_distanceMetrics->AddMethod(_dm, _l);
    }
 
 
    ValidityCheckerPointer GetValidityChecker(const std::string& _l) {
      return m_validityCheckers->GetMethod(_l);
    }
    void AddValidityChecker(ValidityCheckerPointer _vc, const std::string& _l) {
      m_validityCheckers->AddMethod(_vc, _l);
    }
 
    // Toggle (negate) the validity output for ALL validity checkers.
    void ToggleValidity() {
      for(auto& vc : *m_validityCheckers)
        vc.second->ToggleValidity();
    }
 
 
    NeighborhoodFinderPointer GetNeighborhoodFinder(const std::string& _l) {
      return m_neighborhoodFinders->GetMethod(_l);
    }
    void AddNeighborhoodFinder(NeighborhoodFinderPointer _nf, const std::string& _l) {
      m_neighborhoodFinders->AddMethod(_nf, _l);
    }
 
 
    const SamplerSet* const GetSamplers() const {return m_samplers;}
    SamplerPointer GetSampler(const std::string& _l) {
      return m_samplers->GetMethod(_l);
    }
    void AddSampler(SamplerPointer _s, const std::string& _l) {
      m_samplers->AddMethod(_s, _l);
    }
 
 
    LocalPlannerPointer GetLocalPlanner(const std::string& _l) {
      return m_localPlanners->GetMethod(_l);
    }
    void AddLocalPlanner(LocalPlannerPointer _lp, const std::string& _l) {
      m_localPlanners->AddMethod(_lp, _l);
    }
 
 
    ExtenderPointer GetExtender(const std::string& _l) {
      return m_extenders->GetMethod(_l);
    }
    void AddExtender(ExtenderPointer _mps, const std::string& _l) {
      m_extenders->AddMethod(_mps, _l);
    }
 
 
    PathModifierPointer GetPathModifier(const std::string& _l) {
      return m_pathModifiers->GetMethod(_l);
    }
    void AddPathModifier(PathModifierPointer _ps, const std::string& _l) {
      m_pathModifiers->AddMethod(_ps, _l);
    }
 
 
    EdgeValidityCheckerPointer GetEdgeValidityChecker(const std::string& _l) {
      return m_edgeValidityCheckers->GetMethod(_l);
    }
    void AddEdgeValidityChecker(EdgeValidityCheckerPointer _ps, const std::string& _l) {
      m_edgeValidityCheckers->AddMethod(_ps, _l);
    }
 
 
    ConnectorPointer GetConnector(const std::string& _l) {
      return m_connectors->GetMethod(_l);
    }
    void AddConnector(ConnectorPointer _c, const std::string& _l) {
      m_connectors->AddMethod(_c, _l);
    }
 
 
    MetricPointer GetMetric(const std::string& _l) {return m_metrics->GetMethod(_l);}
    void AddMetric(MetricPointer _m, const std::string& _l) {
      m_metrics->AddMethod(_m, _l);
    }
 
 
    MapEvaluatorPointer GetMapEvaluator(const std::string& _l) {
      return m_mapEvaluators->GetMethod(_l);
    }
    void AddMapEvaluator(MapEvaluatorPointer _me, const std::string& _l) {
      m_mapEvaluators->AddMethod(_me, _l);
    }
 
/*    void ResetTimeEvaluators() {
      for(auto& labelPtr : *m_mapEvaluators) {
        auto t = dynamic_cast<TimeEvaluator*>(labelPtr.second.get());
        if(t)
          t->Initialize();
      }
    }
*/
 
    const MPStrategySet* const GetMPStrategies() const {return m_mpStrategies;}
    MPStrategyPointer GetMPStrategy(const std::string& _l) {
      return m_mpStrategies->GetMethod(_l);
    }
    void AddMPStrategy(MPStrategyPointer _mps, const std::string& _l) {
      m_mpStrategies->AddMethod(_mps, _l);
    }
 
 
    MPTools* GetMPTools() {return m_mpTools;}
 
 
    MPProblem* GetMPProblem() const noexcept;
    void SetMPProblem(MPProblem* const _problem) noexcept;
 
    MPTask* GetTask() const noexcept;
    void SetTask(MPTask* const _task) noexcept;
 
    GroupTask* GetGroupTask() const noexcept;
    void SetGroupTask(GroupTask* const _task) noexcept;
 
    const std::string& GetBaseFilename() const noexcept;
    void SetBaseFilename(const std::string& _s) noexcept;
 
 
    MPSolution* GetMPSolution() const noexcept;
    void SetMPSolution(MPSolution* _sol) noexcept;
 
    RoadmapType*      GetRoadmap(Robot* const _r = nullptr) const noexcept;
    GroupRoadmapType* GetGroupRoadmap(RobotGroup* const _g = nullptr) const noexcept;
    RoadmapType*      GetBlockRoadmap(Robot* const _r = nullptr) const noexcept;
    Path*             GetPath(Robot* const _r = nullptr) const noexcept;
    GroupPath*        GetGroupPath(RobotGroup* const _g = nullptr) const noexcept;
    LocalObstacleMap* GetLocalObstacleMap(Robot* const _r = nullptr) const noexcept;
 
    GoalTracker*      GetGoalTracker() const noexcept;
    StatClass*        GetStatClass() const noexcept;
 
 
    std::vector<typename RoadmapType::VP> ReconstructEdge(
        RoadmapType* const _roadmap,
        const VID _source, const VID _target,
        const double _posRes, const double _oriRes);
 
    std::vector<typename GroupRoadmapType::VP> ReconstructEdge(
        GroupRoadmapType* const _roadmap,
        const VID _source, const VID _target,
        const double _posRes, const double _oriRes);
 
    template <typename AbstractRoadmapType>
    std::vector<typename AbstractRoadmapType::VP> ReconstructEdge(
        AbstractRoadmapType* const _roadmap,
        const VID _source, const VID _target);
 
 
    void Halt();
 
    bool IsRunning() const noexcept;
 
    void SetSeed() const noexcept;
 
    void SetSeed(const long _seed) const noexcept;
 
    void AddSolver(const std::string& _label, long _seed,
        const std::string& _baseFileName, bool _vizmoDebug = false) {
      m_solvers.push_back(Solver{_label, _seed, _baseFileName, _vizmoDebug});
    }
 
    std::vector<std::string> GetSolverLabels() {
      std::vector<std::string> labels;
      for (auto& solver : m_solvers)
        labels.push_back(solver.label);
      return labels;
    }
 
    void Solve(MPProblem* _problem, MPTask* _task, MPSolution* _solution);
 
    void Solve(MPProblem* _problem, MPTask* _task);
 
    void Solve(MPProblem* _problem, GroupTask* _task);
 
    void Solve(MPProblem* _problem, GroupTask* _task, MPSolution* _solution);
 
    void Solve(MPProblem* _problem, MPTask* _task, MPSolution* _solution,
        const std::string& _label, const long _seed,
        const std::string& _baseFilename);
 
 
    void Print(std::ostream& _os) const; 
 
 
  private:
 
 
    void RunSolver(const Solver& _s);
 
 
    void Initialize();
 
    void Uninitialize();
 
    bool ParseChild(XMLNode& _node);
 
 
    MPProblem*          m_problem{nullptr};  
    MPTask*             m_task{nullptr};     
    GroupTask*          m_groupTask{nullptr};
    MPSolution*         m_solution{nullptr}; 
    std::vector<Solver> m_solvers;           
 
 
    std::unique_ptr<GoalTracker> m_goalTracker;
    MPTools* m_mpTools{nullptr};
 
 
    std::atomic<bool> m_running{true};  
 
 
  protected:
 
 
    DistanceMetricSet*      m_distanceMetrics{nullptr};
    ValidityCheckerSet*     m_validityCheckers{nullptr};
    NeighborhoodFinderSet*  m_neighborhoodFinders{nullptr};
    SamplerSet*             m_samplers{nullptr};
    LocalPlannerSet*        m_localPlanners{nullptr};
    ExtenderSet*            m_extenders{nullptr};
    PathModifierSet*        m_pathModifiers{nullptr};
    EdgeValidityCheckerSet* m_edgeValidityCheckers{nullptr};
    ConnectorSet*           m_connectors{nullptr};
    MetricSet*              m_metrics{nullptr};
    MapEvaluatorSet*        m_mapEvaluators{nullptr};
    MPStrategySet*          m_mpStrategies{nullptr};
 
};
 
 
template <typename AbstractRoadmapType>
std::vector<typename AbstractRoadmapType::VP>
MPLibrary::
ReconstructEdge(AbstractRoadmapType* const _roadmap, const VID _source,
    const VID _target) {
  auto env = this->GetMPProblem()->GetEnvironment();
  return this->ReconstructEdge(_roadmap, _source, _target,
                               env->GetPositionRes(), env->GetOrientationRes());
}
 
#endif