Skip to content

Commit

Permalink
update mapd
Browse files Browse the repository at this point in the history
  • Loading branch information
@Kei18
Kei18 committed Aug 17, 2021
1 parent 945177b commit d3c8adc
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 112 deletions.
6 changes: 1 addition & 5 deletions pibt2/include/pibt_mapd.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,7 @@ class PIBT_MAPD : public MAPD_Solver
Agents occupied_next;

// result of priority inheritance: true -> valid, false -> invalid
bool funcPIBT(Agent* ai);
// plan next node
Node* planOneStep(Agent* a);
// chose one node from candidates, used in planOneStep
Node* chooseNode(Agent* a);
bool funcPIBT(Agent* ai, Agent* aj = nullptr);

// main
void run();
Expand Down
12 changes: 6 additions & 6 deletions pibt2/src/pibt.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ void PIBT::run()
if (a->init_d != b->init_d) return a->init_d > b->init_d;
return a->tie_breaker > b->tie_breaker;
};
Agents U;
Agents A;

// initialize
for (int i = 0; i < P->getNum(); ++i) {
Expand All @@ -33,7 +33,7 @@ void PIBT::run()
0, // elapsed
d, // dist from s -> g
getRandomFloat(0, 1, MT)}; // tie-breaker
U.push_back(a);
A.push_back(a);
occupied_now[s->id] = a;
}
solution.add(P->getConfigStart());
Expand All @@ -44,8 +44,8 @@ void PIBT::run()
info(" ", "elapsed:", getSolverElapsedTime(), ", timestep:", timestep);

// planning
std::sort(U.begin(), U.end(), compare);
for (auto a : U) {
std::sort(A.begin(), A.end(), compare);
for (auto a : A) {
// if the agent has next location, then skip
if (a->v_next == nullptr) {
// determine its next location
Expand All @@ -56,7 +56,7 @@ void PIBT::run()
// acting
bool check_goal_cond = true;
Config config(P->getNum(), nullptr);
for (auto a : U) {
for (auto a : A) {
// clear
if (occupied_now[a->v_now->id] == a) occupied_now[a->v_now->id] = nullptr;
occupied_next[a->v_next->id] = nullptr;
Expand Down Expand Up @@ -90,7 +90,7 @@ void PIBT::run()
}

// memory clear
for (auto a : U) delete a;
for (auto a : A) delete a;
}

bool PIBT::funcPIBT(Agent* ai, Agent* aj)
Expand Down
154 changes: 53 additions & 101 deletions pibt2/src/pibt_mapd.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,20 +14,14 @@ void PIBT_MAPD::run()
{
// compare priority of agents
auto compare = [](Agent* a, const Agent* b) {
if (a->task != nullptr && b->task == nullptr) return false;
if (a->task == nullptr && b->task != nullptr) return true;
if (a->task != nullptr && b->task == nullptr) return true;
if (a->task == nullptr && b->task != nullptr) return false;

if (a->elapsed != b->elapsed) return a->elapsed < b->elapsed;
if (a->elapsed != b->elapsed) return a->elapsed > b->elapsed;
// use initial distance
return a->tie_breaker < b->tie_breaker;
return a->tie_breaker > b->tie_breaker;
};

// agents have not decided their next locations
std::priority_queue<Agent*, Agents, decltype(compare)> undecided(compare);
// agents have already decided their next locations
std::vector<Agent*> decided;
// whole agents
std::vector<Agent*> A;
Agents A;

// initialize
for (int i = 0; i < P->getNum(); ++i) {
Expand All @@ -42,9 +36,8 @@ void PIBT_MAPD::run()
nullptr, // task (assigned)
nullptr, // target_task (if free)
};
undecided.push(a);
occupied_now[s->id] = a;
A.push_back(a);
occupied_now[s->id] = a;
}
solution.add(P->getConfigStart());

Expand Down Expand Up @@ -119,22 +112,20 @@ void PIBT_MAPD::run()
}

// planning
while (!undecided.empty()) {
// pickup the agent with highest priority
Agent* a = undecided.top();
undecided.pop();

// if the agent has next location, then skip
if (a->v_next == nullptr) {
// determine its next location
funcPIBT(a);
{
std::sort(A.begin(), A.end(), compare);
for (auto a : A) {
// if the agent has next location, then skip
if (a->v_next == nullptr) {
// determine its next location
funcPIBT(a);
}
}
decided.push_back(a);
}

// acting
Config config(P->getNum(), nullptr);
for (auto a : decided) {
for (auto a : A) {
// clear
if (occupied_now[a->v_now->id] == a) occupied_now[a->v_now->id] = nullptr;
occupied_next[a->v_next->id] = nullptr;
Expand All @@ -147,8 +138,6 @@ void PIBT_MAPD::run()
// reset params
a->v_now = a->v_next;
a->v_next = nullptr;
// push to priority queue
undecided.push(a);

// update task info
if (a->task != nullptr) { // assigned agent
Expand All @@ -169,7 +158,6 @@ void PIBT_MAPD::run()
}
}
}
decided.clear();

// update plan
solution.add(config);
Expand All @@ -192,11 +180,11 @@ void PIBT_MAPD::run()

// align target history and plan
{
Nodes targets;
Tasks tasks;
Nodes targets(P->getNum(), nullptr);
Tasks tasks(P->getNum(), nullptr);
for (auto a : A) {
targets.push_back(a->g);
tasks.push_back(a->task);
targets[a->id] = a->g;
tasks[a->id] = a->task;
}
hist_targets.push_back(targets);
hist_tasks.push_back(tasks);
Expand All @@ -206,84 +194,48 @@ void PIBT_MAPD::run()
for (auto a : A) delete a;
}

bool PIBT_MAPD::funcPIBT(Agent* ai)
{
// decide next node
Node* v = planOneStep(ai);
while (v != nullptr) {
auto aj = occupied_now[v->id];
if (aj != nullptr) { // someone occupies v
// avoid itself && allow rotations
if (aj != ai && aj->v_next == nullptr) {
// do priority inheritance and backtracking
if (!funcPIBT(aj)) {
// replan
v = planOneStep(ai);
continue;
}
}
}
// success to plan next one step
return true;
}
// failed to secure node, cope stuck
occupied_next[ai->v_now->id] = ai;
ai->v_next = ai->v_now;
return false;
}

/*
* no candidate node -> return nullptr
*/
Node* PIBT_MAPD::planOneStep(Agent* a)
bool PIBT_MAPD::funcPIBT(Agent* ai, Agent* aj)
{
Node* v = chooseNode(a);
if (v != nullptr) {
// update reservation
occupied_next[v->id] = a;
a->v_next = v;
}
return v;
}

// no candidate node -> return nullptr
Node* PIBT_MAPD::chooseNode(Agent* a)
{
// candidates
Nodes C = a->v_now->neighbor;
C.push_back(a->v_now);

// randomize
std::shuffle(C.begin(), C.end(), *MT);
// compare two nodes
auto compare = [&] (Node* const v, Node* const u) {
int d_v = pathDist(v, ai->g);
int d_u = pathDist(u, ai->g);
if (d_v != d_u) return d_v < d_u;
// tie break
if (occupied_now[v->id] != nullptr
&& occupied_now[u->id] == nullptr) return false;
if (occupied_now[v->id] == nullptr
&& occupied_now[u->id] != nullptr) return true;
// randomize
return getRandomBoolean(MT);
};

// desired node
Node* v = nullptr;
// get candidates
Nodes C = ai->v_now->neighbor;
C.push_back(ai->v_now);
std::sort(C.begin(), C.end(), compare);

// pickup one node
for (auto u : C) {
// avoid vertex conflict
// avoid conflicts
if (occupied_next[u->id] != nullptr) continue;
// avoid swap conflict
auto a_j = occupied_now[u->id];
if (a_j != nullptr && a_j->v_next == a->v_now) continue;

// goal exists -> return immediately
if (u == a->g) return u;

// determine the next node
if (v == nullptr) {
v = u;
} else {
int c_v = pathDist(v, a->g);
int c_u = pathDist(u, a->g);
if ((c_u < c_v) || (c_u == c_v && occupied_now[v->id] != nullptr &&
occupied_now[u->id] == nullptr)) {
v = u;
}
if (aj != nullptr && u == aj->v_now) continue;

// reserve
occupied_next[u->id] = ai;
ai->v_next = u;

auto ak = occupied_now[u->id];
if (ak != nullptr && ak->v_next == nullptr) {
if (!funcPIBT(ak, ai)) continue; // replanning
}
// success to plan next one step
return true;
}

return v;
// failed to secure node
occupied_next[ai->v_now->id] = ai;
ai->v_next = ai->v_now;
return false;
}

void PIBT_MAPD::printHelp() { printHelpWithoutOption(SOLVER_NAME); }

0 comments on commit d3c8adc

Please sign in to comment.