Spaces:
Runtime error
Runtime error
File size: 6,383 Bytes
ad7641a | 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 | from globals import *
from functions import *
from functions_plotting import *
from algs.alg_mapf_pibt import run_procedure_pibt
from algs.alg_functions_lacam import get_init_order, get_order, get_config_name
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# CLASSES
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
class LowLevelNodeStar:
def __init__(self,
parent: Self | None,
who: AgentAlg | None,
where: Node | None,
depth: int):
self.parent: Self | None = parent
self.who: AgentAlg | None = who
self.where: Node | None = where
self.depth = depth
self.who_list: List[AgentAlg] = []
self.where_list: List[Node] = []
def __str__(self):
return f'~ depth={self.depth}, who_list={self.who_list}, where_list={self.where_list} ~'
def __repr__(self):
return f'~ depth={self.depth}, who_list={self.who_list}, where_list={self.where_list} ~'
class HighLevelNodeStar:
def __init__(
self,
config: Dict[str, Node],
tree: Deque[LowLevelNodeStar],
order: List[AgentAlg],
parent: Self | None,
g: int = 0,
h: int = 0,
finished: int = 0,
):
self.config: Dict[str, Node] = config
self.tree: Deque[LowLevelNodeStar] = tree
self.order: List[AgentAlg] = order
self.parent: Self | None = parent
self.finished: int = finished
self.name = get_config_name(self.config)
self.g: int = g
self.h: int = h
self.f: int = self.g + self.h
self.neigh: Set[Self] = set()
def __eq__(self, other: Self):
return self.name == other.name
def __str__(self):
return self.name
def __repr__(self):
return self.name
def __hash__(self):
return self.name.__hash__()
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# HELP FUNCS
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
# -------------------------------------------------------------------------------------------------------------------- #
def get_C_init():
return LowLevelNodeStar(None, None, None, 0)
def get_C_child(
parent: LowLevelNodeStar,
who: AgentAlg,
where: Node
) -> LowLevelNodeStar:
C_new = LowLevelNodeStar(parent=parent, who=who, where=where, depth=parent.depth + 1)
C_new.who_list = parent.who_list + [who]
C_new.where_list = parent.where_list + [where]
return C_new
def backtrack(N: HighLevelNodeStar) -> Dict[str, List[Node]]:
paths_deque_dict: Dict[str, Deque[Node]] = {k: deque([v]) for k, v in N.config.items()}
parent: HighLevelNodeStar = N.parent
while parent is not None:
for k, v in parent.config.items():
paths_deque_dict[k].appendleft(v)
parent = parent.parent
paths_dict: Dict[str, List[Node]] = {}
for k, v in paths_deque_dict.items():
paths_dict[k] = list(v)
return paths_dict
def get_new_config(
N: HighLevelNodeStar,
C: LowLevelNodeStar,
agents_dict: Dict[str, AgentAlg],
nodes_dict: Dict[str, Node],
h_dict: Dict[str, np.ndarray],
iteration: int = 0,
) -> Dict[str, Node] | None:
# setup next configuration
config_from: Dict[str, Node] = N.config
occupied_from: Dict[str, AgentAlg] = {v.xy_name: agents_dict[k] for k, v in N.config.items()}
config_to: Dict[str, Node] = {}
occupied_to: Dict[str, AgentAlg] = {}
for k in range(C.depth):
agent = C.who_list[k]
node = C.where_list[k]
config_to[agent.name] = node
# vc
if node.xy_name in occupied_to:
return None
occupied_to[node.xy_name] = agent
# ec
if node.xy_name in occupied_from:
other_agent = occupied_from[node.xy_name]
if other_agent != agent and other_agent.name in config_to and config_to[other_agent.name] == config_from[agent.name]:
return None
# apply PIBT
for agent in N.order:
if agent.name not in config_to:
success = run_procedure_pibt(
agent,
config_from, occupied_from,
config_to, occupied_to,
agents_dict, nodes_dict, h_dict, [],
with_swap=True,
# with_swap=False,
iteration=iteration
)
if not success:
return None
return config_to
def get_edge_cost(
agents: List[AgentAlg],
config_from: Dict[str, Node],
config_to: Dict[str, Node],
):
# e.g., \sum_i | not (Q_from[i] == Q_to[k] == g_i) |
cost = 0
for agent in agents:
if not (agent.goal_node == config_from[agent.name] == config_to[agent.name]):
cost += 1
return cost
def get_h_value(
config: Dict[str, Node],
h_dict: Dict[str, np.ndarray],
agents: List[AgentAlg]
) -> int:
# e.g., \sum_i dist(Q[i], g_i)
cost = 0
for agent in agents:
goal_np = h_dict[agent.goal_node_name]
curr_n = config[agent.name]
c: float = float(goal_np[curr_n.x, curr_n.y])
cost += c
return cost
|