grstaps/src/solver_fcpop.cpp at master · amessing/grstaps · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#include "grstaps/solver_fcpop.hpp"

#include "grstaps/Task_Allocation/TaskAllocation.h"
#include "grstaps/task_planning/planner_parameters.hpp"
#include "grstaps/task_planning/task_planner.hpp"
#include "grstaps/task_planning/setup.hpp"
#include "grstaps/logger.hpp"
#include "grstaps/problem.hpp"
#include "grstaps/timer.hpp"

namespace grstaps
{
    void planSubcomponents(const Plan *base, std::vector<const Plan *> &plan_subcomponents)
    {
        if(base == nullptr)
        {
            plan_subcomponents.clear();
        }
        else
        {
            planSubcomponents(base->parentPlan, plan_subcomponents);
            plan_subcomponents.push_back(base);
        }
    }

    nlohmann::json writeSolution(TaskPlanner& task_planner, Plan* p, SASTask* task)
    {
        nlohmann::json rv;

        std::vector<const Plan*> plan_subcomponents;
        planSubcomponents(p, plan_subcomponents);
        std::vector<std::string> actions;
        for(const Plan* subcomponent: plan_subcomponents)
        {
            actions.push_back(subcomponent->action->name);
        }
        rv["actions"] = actions;
        rv["schedule"] = task_planner.scheduleAsJson(p, task);

        return rv;
    }

    nlohmann::json SolverFcpop::solve(const std::string& domain_filepath, const std::string& problem_filepath)
    {
        Logger::debug("start");
        // Preprocess pddl
        PlannerParameters parameters;
        parameters.domainFileName  = domain_filepath.c_str();
        parameters.problemFileName = problem_filepath.c_str();
//        {
//            parameters.outputFileName    = "problems/0/0/output";
            parameters.generateMutexFile = true;
            parameters.generateTrace = true;
            parameters.generateGroundedDomain = true;
//        }
        Timer timer;
        timer.start();
        auto task = Setup::doPreprocess(&parameters);
        timer.stop();
        float preprocess_time = timer.get();
        timer.reset();
        Logger::debug("Grounded Actions: {}", task->actions.size());

        // Task planner
        TaskPlanner task_planner(task);
        m_tp_nodes_expanded = 0;
        m_tp_nodes_visited  = 0;
        float num_branches = 0;
        float num_times_branched = 0;

        timer.start();
        Plan* base;
        while(!task_planner.emptySearchSpace())
        {
            base = task_planner.poll();
            if(base->isSolution())
            {
                timer.stop();
                nlohmann::json metrics = {
                    {"makespan", task_planner.getMakespan(base)},
                    {"total_grounded_actions", task->actions.size()},
                    {"num_actions", task_planner.getPlanActions(base)},
                    {"num_tp_nodes_expanded", m_tp_nodes_expanded},
                    {"num_tp_nodes_visited", m_tp_nodes_visited},
                    {"avg_branching_factor", num_branches / num_times_branched},
                    {"timer", timer.get()},
                    {"preprocess_timer", preprocess_time},
                    {"solution", writeSolution(task_planner, base, task)}
                };
                return metrics;
            }

            ++m_tp_nodes_expanded;
            Logger::debug("Expanding plan: {}", base->id);
            std::vector<Plan*> successors = task_planner.getNextSuccessors(base);
            ++num_times_branched;
            for(Plan* plan : successors)
            {
                m_tp_nodes_visited += successors.size();
                num_branches += successors.size();
                task_planner.update(base, successors);
            }
        }

        return nlohmann::json();
    }
}