MPSolution.h

Go to the documentation of this file.

#ifndef PMPL_MP_SOLUTION_TYPE_H_
#define PMPL_MP_SOLUTION_TYPE_H_
 
#include "ConfigurationSpace/LocalObstacleMap.h"
#include "ConfigurationSpace/GenericStateGraph.h"
#include "ConfigurationSpace/GroupRoadmap.h"
#include "ConfigurationSpace/GroupPath.h"
#include "ConfigurationSpace/Path.h"
#include "MPProblem/Robot/Robot.h"
#include "MPProblem/RobotGroup/RobotGroup.h"
#include "Utilities/MetricUtils.h"
 
// #include <map>
// #include <memory>
// #include <unordered_map>
 
class Robot;
class Path;
class GroupPath;
 
 
class MPSolutionType final {
 
  public:
 
 
    typedef GenericStateGraph<Cfg, DefaultWeight<Cfg>> RoadmapType;
    typedef GroupCfg<RoadmapType> GroupCfgType;
    typedef GroupRoadmap<GroupCfgType, GroupLocalPlan<RoadmapType>> GroupRoadmapType;
 
    struct RobotSolution {
      std::unique_ptr<RoadmapType>  freeMap; 
      std::unique_ptr<RoadmapType>  obstMap; 
      std::unique_ptr<Path>            path; 
      std::unique_ptr<LocalObstacleMap> lom; 
 
      RobotSolution() = default;
 
      RobotSolution(Robot* const _robot, StatClass* const _stats);
    };
 
    struct GroupSolution {
      std::unique_ptr<GroupRoadmapType>  freeMap; 
      std::unique_ptr<GroupRoadmapType>  obstMap; 
      std::unique_ptr<GroupPath>        path; 
 
      GroupSolution() = default;
 
      GroupSolution(RobotGroup* const _group, MPSolutionType* const _solution);
    };
 
 
    MPSolutionType(Robot* const _r);
 
    MPSolutionType(RobotGroup* const _g);
 
 
    void AddRobot(Robot* const _r) noexcept;
 
    void SetRobot(Robot* const _r) noexcept;
 
    void SetRoadmap(Robot* const _r, RoadmapType* _roadmap) noexcept;
 
    void SetPath(Robot* const _r, Path* _path) noexcept;
 
    void AddRobotGroup(RobotGroup* const _g) noexcept;
 
    void SetGroupRoadmap(RobotGroup* const _g, GroupRoadmapType* _roadmap) noexcept;
 
    void SetGroupPath(RobotGroup* const _r, GroupPath* _path) noexcept;
 
 
    RoadmapType* GetRoadmap(Robot* const _r = nullptr) const noexcept;
 
    RoadmapType* GetBlockRoadmap(Robot* const _r = nullptr) const noexcept;
 
    Path* GetPath(Robot* const _r = nullptr) const noexcept;
 
    LocalObstacleMap* GetLocalObstacleMap(Robot* const _r = nullptr) const
        noexcept;
 
    GroupRoadmapType* GetGroupRoadmap(RobotGroup* const _g = nullptr) const
        noexcept;
 
    GroupPath* GetGroupPath(RobotGroup* const _g = nullptr) const noexcept;
 
    StatClass* GetStatClass() const noexcept;
 
    Robot* GetRobot() const noexcept;
 
    RobotGroup* GetRobotGroup() const noexcept;
 
 
  private:
 
 
    const RobotSolution* GetRobotSolution(Robot* _r) const noexcept;
 
    RobotSolution* GetRobotSolution(Robot* _r) noexcept;
 
    const GroupSolution* GetGroupSolution(RobotGroup* _g) const noexcept;
 
    GroupSolution* GetGroupSolution(RobotGroup* _g) noexcept;
 
    Robot* m_robot{nullptr};            
    RobotGroup* m_group{nullptr};       
 
    std::unique_ptr<StatClass> m_stats; 
 
    std::unordered_map<Robot*, RobotSolution> m_individualSolutions;
    std::unordered_map<RobotGroup*, GroupSolution> m_groupSolutions;
 
 
};
 
#endif