Skip to content

Commit

Permalink
add comment for CBS & ICBS refine
Browse files Browse the repository at this point in the history
  • Loading branch information
@Kei18
Kei18 committed Aug 10, 2020
1 parent 66680bc commit fe506cd
Show file tree
Hide file tree
Showing 5 changed files with 49 additions and 34 deletions.
20 changes: 11 additions & 9 deletions mapf/include/cbs_refine.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,22 @@
/*
* Implementation of Conflict-based Search (CBS)
* for Iterative Refinement
* Implementation of CBS for Iterative Refinement
* This cannot be used directly
*/

#pragma once
#include "cbs.hpp"

class CBS_REFINE : public virtual CBS {
protected:
const Plan old_plan;
const Paths old_paths;
const int ub_makespan;
const int ub_soc;
const std::vector<int> sample;
std::vector<int> fixed_agents;
const Plan old_plan; // old plan, equivalent to old_paths
const Paths old_paths; // old paths, equivalent to old_plan
const int ub_makespan; // makespan in the old plan
const int ub_soc; // sum of costs in the old plan
const std::vector<int> modif_list; // a modification list M
std::vector<int> fixed_agents; // A \ modif_list

// true -> makespan optimization, default: false
// notice: SOC and makespan are Pareto structure.
bool makespan_prioritized;

virtual void setInitialHighLevelNode(HighLevelNode_p n);
Expand All @@ -25,7 +27,7 @@ class CBS_REFINE : public virtual CBS {
public:
CBS_REFINE(Problem* _P,
const Plan& _old_plan,
const std::vector<int>& _sample);
const std::vector<int>& _modif_list);
~CBS_REFINE() {};

void setParams(int argc, char *argv[]);
Expand Down
6 changes: 3 additions & 3 deletions mapf/include/icbs_refine.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/*
* Implementation of Improved Conflict-based Search (ICBS)
* for Iterative Refinement
* Implementation of ICBS for Iterative Refinement
* Do not use this directly
*/

#pragma once
Expand All @@ -16,6 +16,6 @@ class ICBS_REFINE : public ICBS, CBS_REFINE {
public:
ICBS_REFINE(Problem* _P,
const Plan& _old_plan,
const std::vector<int>& _sample);
const std::vector<int>& _modif_list);
~ICBS_REFINE() {};
};
4 changes: 4 additions & 0 deletions mapf/include/pibt_complete.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,7 @@
/*
* Implementation of PIBT-Complete
*/

#pragma once
#include "solver.hpp"

Expand Down
37 changes: 22 additions & 15 deletions mapf/src/cbs_refine.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,37 +2,42 @@

CBS_REFINE::CBS_REFINE(Problem* _P,
const Plan& _old_plan,
const std::vector<int>& _sample)
const std::vector<int>& _modif_list)
: CBS(_P),
old_plan(_old_plan),
old_paths(planToPaths(_old_plan)),
ub_makespan(_old_plan.getMakespan()),
ub_soc(_old_plan.getSOC()),
sample(_sample),
makespan_prioritized(true)
modif_list(_modif_list),
makespan_prioritized(false)
{
if (!sample.empty()) {
// create fixed_agents set
if (!modif_list.empty()) {
if (_old_plan.empty()) return;
for (int i = 0; i < _old_plan.get(0).size(); ++i) {
if (!inArray(i, sample)) fixed_agents.push_back(i);
if (!inArray(i, modif_list)) fixed_agents.push_back(i);
}
}
}

void CBS_REFINE::setInitialHighLevelNode(HighLevelNode_p n)
{
if (!sample.empty()) {
if (!modif_list.empty()) {
// create paths only for agents in modif_list
Paths paths(P->getNum());
for (int i = 0; i < P->getNum(); ++i) {
if (inArray(i, sample)) {
if (inArray(i, modif_list)) {
// agents in modif_list
Path path = CBS_REFINE::getInitialPath(i);
if (path.empty()) {
if (path.empty()) { // failed
n->valid = false;
return;
}
paths.insert(i, path);
} else {
paths.insert(i, old_paths.get(i)); // fixed agents
// for fixed agents
// use the old path
paths.insert(i, old_paths.get(i));
}
}
n->paths = paths;
Expand All @@ -47,6 +52,7 @@ Path CBS_REFINE::getInitialPath(int id)
Node* g = P->getGoal(id);
Nodes config_g = P->getConfigGoal();

// pre processing
int max_constraint_time = 0;
for (auto i : fixed_agents) {
for (int t = 1; t <= ub_makespan; ++t) {
Expand All @@ -62,14 +68,13 @@ Path CBS_REFINE::getInitialPath(int id)
} else {
fValue = [&] (AstarNode* n) {
return std::max(max_constraint_time + 1,
n->g + pathDist(n->v, g));
};
n->g + pathDist(n->v, g)); };
}

CompareAstarNode compare =
[&] (AstarNode* a, AstarNode* b) {
if (a->f != b->f) return a->f > b->f;
// [IMPORTANT!] avoid goal locations of others
// IMPORTANT! avoid goal locations of others
if (a->v != g && inArray(a->v, config_g)) return true;
if (b->v != g && inArray(b->v, config_g)) return false;
if (a->g != b->g) return a->g < b->g;
Expand All @@ -84,11 +89,12 @@ Path CBS_REFINE::getInitialPath(int id)
CheckInvalidAstarNode checkInvalidAstarNode =
[&] (AstarNode* m) {
if (m->g > ub_makespan) {
// cut off low-level nodes
if (makespan_prioritized) return true;
if (inArray(m->v, config_g) && m->v != g) return true;
return false;
}
if (makespan_prioritized && m->g > ub_makespan) return true;
// see conflicts with fixed agents
for (auto i : fixed_agents) {
// vertex conflicts
if (m->v == old_paths.get(i, m->g)) return true;
Expand All @@ -114,7 +120,7 @@ CBS::CompareHighLevelNodes CBS_REFINE::getObjective()
if (makespan_prioritized && a->makespan != b->makespan)
return a->makespan > b->makespan;
if (a->soc != b->soc) return a->soc > b->soc;
if (a->f != b->f) return a->f > b->f; // tie-breaker
if (a->f != b->f) return a->f > b->f;
return false;
};
return compare;
Expand All @@ -125,6 +131,7 @@ Path CBS_REFINE::getConstrainedPath(HighLevelNode_p h_node, int id)
Node* s = P->getStart(id);
Node* g = P->getGoal(id);

// pre processing
LibCBS::Constraints constraints;
int max_constraint_time = 0;
for (auto c : h_node->constraints) {
Expand Down Expand Up @@ -214,7 +221,7 @@ void CBS_REFINE::setParams(int argc, char *argv[])
longopts, &longindex)) != -1) {
switch (opt) {
case 'p':
makespan_prioritized = false;
makespan_prioritized = true;
break;
default:
break;
Expand Down
16 changes: 9 additions & 7 deletions mapf/src/icbs_refine.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,15 @@

ICBS_REFINE::ICBS_REFINE(Problem* _P,
const Plan& _old_plan,
const std::vector<int>& _sample)
const std::vector<int>& _modif_list)
: CBS(_P), ICBS(_P),
CBS_REFINE(_P, _old_plan, _sample)
CBS_REFINE(_P, _old_plan, _modif_list)
{
}

void ICBS_REFINE::setInitialHighLevelNode(HighLevelNode_p n)
{
if (sample.empty()) {
if (modif_list.empty()) {
ICBS::setInitialHighLevelNode(n);
return;
}
Expand All @@ -33,7 +33,8 @@ void ICBS_REFINE::setInitialHighLevelNode(HighLevelNode_p n)
}
Path path;
LibCBS::MDD_p mdd;
if (inArray(i, sample)) {
if (inArray(i, modif_list)) {
// find path using A* search
path = CBS_REFINE::getInitialPath(i);
if (path.empty()) {
n->valid = false;
Expand All @@ -45,6 +46,7 @@ void ICBS_REFINE::setInitialHighLevelNode(HighLevelNode_p n)
n->valid = false;
return;
}
// create mdd
mdd = std::make_shared<LibCBS::MDD>
(LibCBS::MDD(path.size()-1, i, P, constraints, time_limit));
if (!mdd->valid) {
Expand All @@ -63,21 +65,21 @@ void ICBS_REFINE::setInitialHighLevelNode(HighLevelNode_p n)
n->constraints = constraints;
n->makespan = paths.getMakespan();
n->soc = paths.getSOC();
n->f = paths.countConflict(sample);
n->f = paths.countConflict(modif_list);
n->valid = true; // valid
MDDTable[n->id] = mdds;
}

LibCBS::Constraints ICBS_REFINE::getPrioritizedConflict
(HighLevelNode_p h_node)
{
if (sample.empty()) {
if (modif_list.empty()) {
return LibCBS::getPrioritizedConflict(h_node->paths,
MDDTable[h_node->id]);
}
return LibCBS::getPrioritizedConflict(h_node->paths,
MDDTable[h_node->id],
sample);
modif_list);
}

// using MDD
Expand Down

0 comments on commit fe506cd

Please sign in to comment.