-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathICBSSearch.h
244 lines (210 loc) · 12.1 KB
/
ICBSSearch.h
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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
// ICBS Search (High-level)
#pragma once
#define USE_GUROBI
//#define GUROBI_WITH_GOAL_CONSTRAINTS
#include <chrono>
#ifdef USE_GUROBI
#include <gurobi_c++.h>
#endif
#include "ICBSNode.h"
#include "ICBSSingleAgentLLSearch.h"
#include "heuristic_calculator.h"
#include "agents_loader.h"
#include "conflict_avoidance_table.h"
#include "XytHolder.h"
class ICBSSearch
{
public:
//settings
split_strategy split;
bool posConstraintsAlsoAddPosConstraintsOnMddNarrowLevelsLeadingToThem = false;
bool preferFCardinals = true;
bool preferGoalConflicts = true;
highlevel_heuristic HL_heuristic;
double focal_w = 1.0;
int child_pref_budget;
int max_child_pref_options;
int time_limit;
// Used to ease tracking of the order of nodes in iterative deepening runs
uint64_t HL_num_generated_before_this_iteration = 0;
uint64_t HL_num_expanded_before_this_iteration = 0;
std::clock_t start; // Lets subroutines check for a timeout
// statistics of efficiency
clock_t runtime = 0; // CPU time, excluding prepTime
clock_t prepTime = 0; // CPU time for initializing CBS and generating the root node
clock_t hlHeuristicTime = 0; // CPU time for initializing CBS and generating the root node
clock_t lowLevelTime = 0; // CPU time of running the low level
clock_t highLevelTime = 0; // CPU time of running the high level
clock_t highLevelMddBuildingTime = 0; // CPU time of building MDDs in the high level
clock_t up_and_down_runtime = 0; // CPU time
clock_t cat_runtime = 0; // CPU time
clock_t hl_node_verification_runtime = 0; // CPU time
std::chrono::nanoseconds wall_runtime = std::chrono::nanoseconds::zero(); // Wall time, excluding prepTime
std::chrono::nanoseconds wall_prepTime = std::chrono::nanoseconds::zero(); // Wall time for initializing CBS and generating the root node
std::chrono::nanoseconds wall_hlHeuristicTime = std::chrono::nanoseconds::zero(); // Wall time for initializing CBS and generating the root node
std::chrono::nanoseconds wall_mddTime = std::chrono::nanoseconds::zero(); // Wall time for building MDDs
std::chrono::nanoseconds wall_lowLevelTime = std::chrono::nanoseconds::zero(); // Wall time of running the low level
std::chrono::nanoseconds wall_highLevelTime = std::chrono::nanoseconds::zero(); // Wall time for running the high level
std::chrono::nanoseconds wall_up_and_down_runtime = std::chrono::nanoseconds::zero(); // Wall time, excluding preprocessing
std::chrono::nanoseconds wall_cat_runtime = std::chrono::nanoseconds::zero(); // Wall time, excluding preprocessing
std::chrono::nanoseconds wall_hl_node_verification_runtime = std::chrono::nanoseconds::zero(); // Wall time, excluding preprocessing
uint64_t HL_num_expanded = 0;
uint64_t HL_num_generated = 0;
uint64_t LL_num_expanded = 0;
uint64_t LL_num_generated = 0;
uint64_t HL_num_reexpanded = 0;
uint64_t f_cardinal_conflicts_found = 0;
uint64_t cardinal_goal_conflicts_found = 0;
uint64_t semi_f_cardinal_conflicts_found = 0;
uint64_t cardinal_conflicts_found = 0;
uint64_t semi_cardinal_goal_conflicts_found = 0;
uint64_t semi_cardinal_conflicts_found = 0;
uint64_t non_cardinal_conflicts_found = 0;
uint64_t num_delta_h_0 = 0;
uint64_t num_delta_h_minus_1 = 0;
uint64_t num_delta_h_1 = 0;
int64_t sum_delta_h_minus_2_or_more = 0;
uint64_t num_delta_h_minus_2_or_more = 0;
uint64_t sum_delta_h_2_or_more = 0;
uint64_t num_delta_h_2_or_more = 0;
uint64_t num_delta_g_0 = 0;
uint64_t num_delta_g_1 = 0;
uint64_t sum_delta_g_2_or_more = 0;
uint64_t num_delta_g_2_or_more = 0;
uint64_t num_delta_f_0 = 0;
uint64_t num_delta_f_1 = 0;
uint64_t sum_delta_f_2_or_more = 0;
uint64_t num_delta_f_2_or_more = 0;
uint64_t num_delta_f_0_delta_g_1 = 0;
uint64_t num_delta_f_0_delta_g_0 = 0;
// Other combinations possible with stronger heuristics
uint64_t num_delta_f_1_delta_g_2 = 0;
uint64_t num_delta_f_1_delta_g_1 = 0;
uint64_t num_delta_f_1_delta_g_0 = 0;
// Other combinations possible with stronger heuristics
uint64_t sum_num_conflicts = 0;
uint64_t num_num_conflicts = 0; // Yes, probably same as num_generated
string max_mem;
// statistics of solution quality
bool solution_found = false;
int solution_cost = -1;
int min_f_val; // In OPEN
double focal_list_threshold;
// For debugging
conflict_type conflictType;
// print
void printPaths(std::ostream& stream, vector<Path *> &paths, int max_len = 100) const;
void printResults() const;
void saveResults(const string& outputFile, const string& agentFile, const string& solver) const;
void isSolutionFeasible();
bool runICBSSearch();
bool runIterativeDeepeningICBSSearch();
ICBSSearch(const MapLoader &ml, const AgentsLoader &al, double focal_w, split_strategy p, highlevel_heuristic HL_h,
int cutoffTime, int child_pref_budget, int max_child_pref_options,
bool propagatePositiveCons, bool preferFCardinals, bool preferGoalConflicts);
~ICBSSearch();
private:
typedef boost::heap::fibonacci_heap< ICBSNode*, boost::heap::compare<ICBSNode::compare_node> > heap_open_t;
typedef boost::heap::fibonacci_heap< ICBSNode*, boost::heap::compare<ICBSNode::secondary_compare_node> > heap_focal_t;
heap_open_t open_list;
heap_focal_t focal_list;
list<ICBSNode*> allNodes_table; // Currently only used for cleaning up. TODO: Consider holding unique_ptrs.
// input
int map_size;
int num_of_agents;
const int* moves_offset;
int num_map_cols;
ConflictAvoidanceTable root_cat;
ICBSNode* root_node;
ICBSNode* solution_node;
vector < ICBSSingleAgentLLSearch* > search_engines; // used to find (single) agents' paths and mdd
vector<Path*> paths; // The paths of the node we're currently working on, also the paths of the solution once it's found
// For best-first-search CBS, this is also the set of paths of the best node in OPEN.
// This is not the case in DFS variants before the solution is found.
// This trades the speed of rebuilding it each time we move to a new node for
// the space of saving it on every node.
// TODO: Why not keep it on every node, and populate it based on the parent?
vector<vector<PathEntry>> paths_found_initially; // contains the initial path that was found for each agent
// print
void printConflicts(std::ostream& stream, const ICBSNode &n) const;
void printConstraints(std::ostream& stream, const ICBSNode* n) const;
//conflicts
void findConflicts(ICBSNode& curr);
void findConflicts(ICBSNode& curr, vector<int>& agents_with_new_paths);
void findConflicts(ICBSNode &curr, int a1, int a2);
void classifyConflicts(ICBSNode &node, ConflictAvoidanceTable* cat = nullptr);
std::shared_ptr<Conflict> getHighestPriorityConflict(ICBSNode &node);
std::shared_ptr<Conflict> getHighestPriorityConflict(ICBSNode &node, const vector<std::shared_ptr<Conflict>> &confs);
void copyConflictsFromParent(ICBSNode& curr);
void clearConflictsOfAgent(ICBSNode &curr, int ag);
void copyConflicts(const vector<bool>& unchanged,
const vector<std::shared_ptr<Conflict>>& from, vector<std::shared_ptr<Conflict>>& to);
void clearConflictsOfAffectedAgents(bool *unchanged, vector<std::shared_ptr<Conflict>> &lst);
void updateConflictCounters(ICBSNode& node);
// branch
void branch(ICBSNode* parent, ICBSNode* child1, ICBSNode* child2);
void disjoint_branch_on_agent(ICBSNode* parent, ICBSNode* child1, ICBSNode* child2, int agent);
bool branch_and_generate_with_up_and_down(ICBSNode* parent, ICBSNode* child1, ICBSNode*child2, ConflictAvoidanceTable *cat);
bool disjoint_branch_and_generate_with_up_and_down(ICBSNode* parent, ICBSNode* child1, ICBSNode* child2,
ConflictAvoidanceTable *cat, int agent_id);
void push_child_into_lists(ICBSNode *child);
bool findPathForSingleAgent(ICBSNode *node, ConflictAvoidanceTable *cat, int newConstraintTimestep,
int earliestGoalTimestep,
int ag, bool skipNewpaths = false);
std::tuple<ICBSNode *, bool> generateChild(ICBSNode *child, ConflictAvoidanceTable *cat = nullptr);
bool finishPartialExpansion(ICBSNode *node, ConflictAvoidanceTable *cat);
int buildConstraintTable(ICBSNode* curr, int agent_id, int newConstraintTimestep,
XytHolder<ConstraintState>& cons_table, pair<int, int>& start, pair<int, int>& goal);
void buildConflictAvoidanceTable(const ICBSNode &node, int exclude_agent, ConflictAvoidanceTable &cat);
void addPathToConflictAvoidanceTable(Path &path, ConflictAvoidanceTable &cat, int agent_id);
void removePathFromConflictAvoidanceTable(Path &path, ConflictAvoidanceTable &cat, int agent_id);
int countMddSingletons(vector<Path *> &paths, int agent_id, int conflict_timestep);
int getTotalMddWidth(vector<Path *> &paths, int agent_id);
void addPositiveConstraintsOnNarrowLevelsLeadingToPositiveConstraint(Path &parent_path,
int new_positive_constraint_timestep, ICBSNode* child, const ConflictAvoidanceTable* cat);
void removePositiveConstraintsOnNarrowLevelsLeadingToPositiveConstraint(Path &parent_path,
int new_positive_constraint_timestep, ICBSNode* child, const ConflictAvoidanceTable* cat);
void findShortestPathFromPrevNodeToCurr(ICBSNode *curr, ICBSNode* prev,
vector<ICBSNode *>& steps_up_from_prev_node_to_lowest_common_ancestor,
vector<ICBSNode *>& steps_down_from_lowest_common_ancestor_to_curr_node);
// update
inline void populatePaths(ICBSNode *curr, vector<Path *> &paths);
void collectConstraints(ICBSNode* curr, std::list<pair<int, tuple<int, int, int, bool>>> &constraints);
bool reinsert(ICBSNode* curr);
void updateFocalList(double old_lower_bound, double new_lower_bound, double f_weight);
// high-level heuristics
int computeHeuristic(ICBSNode& curr);
bool KVertexCover(const vector<vector<bool>>& CG, int num_of_CGnodes, int num_of_CGedges, int k);
#ifdef USE_GUROBI
GRBEnv gurobi_env;
//TEMP:
GRBModel mvc_model;
GRBVar* mvc_vars;
inline void remove_model_constraints(vector<vector<vector<GRBConstr>>>& Constraints, vector<vector<bool>>& CG, vector<bool>& CgNodeDegrees);
inline void calc_mvc_mode_node_degrees(ICBSNode& curr, vector<int>& nodeDegrees);
inline void add_mvc_model_constraints_from_conflicts(vector<shared_ptr<Conflict>>& conflicts, vector<vector<bool>>& CG,
vector<int>& CgNodeDegrees, vector<vector<vector<GRBConstr>>>& Constraints,
int& num_of_nontrivial_CG_edges, vector<bool>& nodeHasNontrivialEdges,
int& h);
inline void re_add_mvc_model_constraints_of_agent(vector<shared_ptr<Conflict>>& conflicts, int i,
vector<vector<vector<GRBConstr>>>& Constraints);
#endif
// tools
bool buildMDD(ICBSNode &curr, int ag, int timestep, int lookahead = 0, ConflictAvoidanceTable *cat = nullptr);
bool arePathsConsistentWithConstraints(vector<Path *> &paths, ICBSNode *curr) const;
inline int getAgentLocation(vector<Path *> &paths, size_t timestep, int agent_id);
std::tuple<bool, int> do_idcbsh_iteration(ICBSNode *curr,
ConflictAvoidanceTable &cat,
int threshold, int next_threshold, clock_t end_by, bool after_bypass = false);
tuple<bool, bool> idcbsh_add_constraint_and_replan(ICBSNode *node,
ConflictAvoidanceTable &cat, int max_cost);
void idcbsh_unconstrain(ICBSNode *node,
ConflictAvoidanceTable &cat,
Path &path_backup,
shared_ptr<Conflict> &conflict_backup, int makespan_backup, int g_val_backup,
int h_val_backup, ICBSNode::WillCostIncrease left_cost_will_increase_backup,
ICBSNode::WillCostIncrease right_cost_will_increase_backup,
bool just_unconstrain = false);
void update_cat_and_lpas(ICBSNode *prev_node, ICBSNode *curr,
vector<Path*>& paths, ConflictAvoidanceTable *cat);
};