Skip to content

modificaitons to handle level 2 json, where no plannerpaths and event… #83

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
nobodyczcz wants to merge 1 commit into
base: dev
Choose a base branch
from
Open

modificaitons to handle level 2 json, where no plannerpaths and event… #83

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 52 additions & 32 deletions script/plan_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1232,9 +1232,13 @@ def load_paths(self, data:Dict):
actual_codes_by_agent = self.extract_agent_codes(
data, "actualPaths", self.team_size, char_to_code, wait_code
)
planner_codes_by_agent = self.extract_agent_codes(
data, "plannerPaths", self.team_size, char_to_code, wait_code
)
has_planner_paths = "plannerPaths" in data
if has_planner_paths:
planner_codes_by_agent = self.extract_agent_codes(
data, "plannerPaths", self.team_size, char_to_code, wait_code
)
else:
planner_codes_by_agent = [np.empty(0, dtype=np.int32)] * self.team_size
if self.window_size is not None:
current_window_end = min(self.start_tstep + self.window_size, self.end_tstep)
else:
Expand Down Expand Up @@ -1282,36 +1286,40 @@ def load_paths(self, data:Dict):
else:
self.exec_paths[ag_id] = np.rint(exec_path_block).astype(np.int32)

plan_step_counts = []
for ag_id in agent_ids:
plan_limit = min(current_window_end, len(self.plan_path_codes[ag_id]))
plan_step_counts.append(max(0, plan_limit - self.start_tstep))
if has_planner_paths:
plan_step_counts = []
for ag_id in agent_ids:
plan_limit = min(current_window_end, len(self.plan_path_codes[ag_id]))
plan_step_counts.append(max(0, plan_limit - self.start_tstep))

plan_motion_batch = self.build_motion_batch(
self.plan_path_codes, agent_ids, [self.start_tstep] * self.team_size, plan_step_counts, wait_code
)
plan_start_states = [self.exec_paths[ag_id][0] for ag_id in agent_ids]
plan_starts_batch = np.zeros((len(plan_start_states), 3), dtype=np.float64)
for row_idx, state in enumerate(plan_start_states):
plan_starts_batch[row_idx, 0] = float(state[0])
plan_starts_batch[row_idx, 1] = float(state[1])
plan_starts_batch[row_idx, 2] = float(state[2])
plan_base_states = self.build_plan_base_state_batch(
agent_ids, [self.start_tstep] * self.team_size, plan_step_counts
)
plan_results = np.zeros((self.team_size, max(plan_step_counts, default=0) + 1, 3), dtype=np.float64)
compute_plan_next_states(
plan_motion_batch, plan_starts_batch, plan_base_states, plan_results,
np.asarray(plan_step_counts, dtype=np.int32),
is_mapf, is_tick, self.ticks_per_timestep
)
plan_motion_batch = self.build_motion_batch(
self.plan_path_codes, agent_ids, [self.start_tstep] * self.team_size, plan_step_counts, wait_code
)
plan_start_states = [self.exec_paths[ag_id][0] for ag_id in agent_ids]
plan_starts_batch = np.zeros((len(plan_start_states), 3), dtype=np.float64)
for row_idx, state in enumerate(plan_start_states):
plan_starts_batch[row_idx, 0] = float(state[0])
plan_starts_batch[row_idx, 1] = float(state[1])
plan_starts_batch[row_idx, 2] = float(state[2])
plan_base_states = self.build_plan_base_state_batch(
agent_ids, [self.start_tstep] * self.team_size, plan_step_counts
)
plan_results = np.zeros((self.team_size, max(plan_step_counts, default=0) + 1, 3), dtype=np.float64)
compute_plan_next_states(
plan_motion_batch, plan_starts_batch, plan_base_states, plan_results,
np.asarray(plan_step_counts, dtype=np.int32),
is_mapf, is_tick, self.ticks_per_timestep
)

for row_idx, ag_id in enumerate(agent_ids):
plan_path_block = plan_results[row_idx, :plan_step_counts[row_idx] + 1]
if is_tick:
self.plan_paths[ag_id] = np.round(plan_path_block, 6)
else:
self.plan_paths[ag_id] = np.rint(plan_path_block).astype(np.int32)
for row_idx, ag_id in enumerate(agent_ids):
plan_path_block = plan_results[row_idx, :plan_step_counts[row_idx] + 1]
if is_tick:
self.plan_paths[ag_id] = np.round(plan_path_block, 6)
else:
self.plan_paths[ag_id] = np.rint(plan_path_block).astype(np.int32)
else:
for ag_id in agent_ids:
self.plan_paths[ag_id] = self.exec_paths[ag_id].copy()

print("Done!")

Expand Down Expand Up @@ -1385,7 +1393,15 @@ def ensure_paths_through(self, target_timestep: int, agent_ids: List[int]=None)
plan_start_indices = []
plan_step_counts = []
for ag_id in agent_ids:
if ag_id not in self.plan_path_codes:
if ag_id not in self.plan_path_codes or len(self.plan_path_codes[ag_id]) == 0:
# No planner data: keep plan_paths in sync with exec_paths
self.plan_paths[ag_id] = self.exec_paths[ag_id].copy()
if ag_id in self.agents:
agent = self.agents[ag_id]
using_plan_path = (agent.path is agent.plan_path)
agent.plan_path = self.plan_paths[ag_id]
if using_plan_path:
agent.path = agent.plan_path
continue
current_plan_end = self.start_tstep + len(self.plan_paths[ag_id]) - 1
plan_limit = min(target_timestep, len(self.plan_path_codes[ag_id]))
Expand Down Expand Up @@ -1591,6 +1607,10 @@ def load_schedule(self, data:Dict):
def load_events(self, data:Dict):
print("Loading event", end="...")

if "events" not in data or not data["events"]:
print("No events.")
return

assert self.max_seq_num > -1
for (finish_tstep, ag_id, task_id, nxt_errand_id) in data["events"]:
if (finish_tstep > self.end_tstep):
Expand Down
34 changes: 23 additions & 11 deletions script/plan_viz.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -749,7 +749,8 @@ def move_agents_per_timestep(self) -> None:
return

prev_aid = max(self.pcf.event_tracker["aid"]-1, 0)
if self.pcf.cur_tstep-1 == self.pcf.event_tracker["aTime"][prev_aid]:
a_times = self.pcf.event_tracker["aTime"]
if a_times and prev_aid < len(a_times) and self.pcf.cur_tstep-1 == a_times[prev_aid]:
# from newly assigned to assigned
for (tid, ag_id) in self.pcf.events["assigned"][self.pcf.cur_tstep-1].items():
self.pcf.tasks[tid].state = "assigned"
Expand All @@ -758,7 +759,8 @@ def move_agents_per_timestep(self) -> None:
if not self.pcf.shown_path_agents or ag_id in self.pcf.shown_path_agents:
self.show_single_task(tid)

if self.pcf.cur_tstep == self.pcf.event_tracker["aTime"][self.pcf.event_tracker["aid"]]:
a_id = self.pcf.event_tracker["aid"]
if a_times and a_id < len(a_times) and self.pcf.cur_tstep == a_times[a_id]:
# from unassigned to newly assigned
for (tid, ag_id) in self.pcf.events["assigned"][self.pcf.cur_tstep].items():
self.pcf.tasks[tid].state = "newlyassigned"
Expand All @@ -768,7 +770,9 @@ def move_agents_per_timestep(self) -> None:
self.show_single_task(tid)
self.pcf.event_tracker["aid"] += 1

if self.pcf.cur_tstep == self.pcf.event_tracker["fTime"][self.pcf.event_tracker["fid"]]:
f_times = self.pcf.event_tracker["fTime"]
f_id = self.pcf.event_tracker["fid"]
if f_times and f_id < len(f_times) and self.pcf.cur_tstep == f_times[f_id]:
# from assigned to finished
for tid in self.pcf.events["finished"][self.pcf.cur_tstep]:
self.pcf.tasks[tid].state = "finished"
Expand All @@ -791,8 +795,10 @@ def back_agents_per_timestep(self) -> None:
# Move the event tracker backward
prev_aid = max(self.pcf.event_tracker["aid"]-1, 0)
prev_fid = max(self.pcf.event_tracker["fid"]-1, 0)
prev_agn_time = self.pcf.event_tracker["aTime"][prev_aid]
prev_fin_time = self.pcf.event_tracker["fTime"][prev_fid]
a_times = self.pcf.event_tracker["aTime"]
f_times = self.pcf.event_tracker["fTime"]
prev_agn_time = a_times[prev_aid] if a_times else -1
prev_fin_time = f_times[prev_fid] if f_times else -1

if self.pcf.cur_tstep == prev_fin_time: # from finished to assigned
for (tid, ag_id) in self.pcf.events["finished"][prev_fin_time].items():
Expand All @@ -813,7 +819,7 @@ def back_agents_per_timestep(self) -> None:
self.show_single_task(tid)
self.pcf.event_tracker["aid"] = prev_aid
prev_aid = max(self.pcf.event_tracker["aid"]-1, 0)
prev_agn_time = self.pcf.event_tracker["aTime"][prev_aid]
prev_agn_time = a_times[prev_aid] if a_times else -1

if prev_timestep == prev_agn_time: # from assigned to newly assigned
for (tid, ag_id) in self.pcf.events["assigned"][prev_agn_time].items():
Expand Down Expand Up @@ -2803,7 +2809,9 @@ def move_agents_per_timestep(self) -> None:
grid_x, grid_y = int(match.group(1)), int(match.group(2))
self.update_location_event_list(self.pop_location_listbox)
self.update_error_list(self.conflict_listbox)
if self.pcf.cur_tstep == self.pcf.event_tracker["aTime"][self.pcf.event_tracker["aid"]]:
a_times = self.pcf.event_tracker["aTime"]
a_id = self.pcf.event_tracker["aid"]
if a_times and a_id < len(a_times) and self.pcf.cur_tstep == a_times[a_id]:
# from unassigned to assigned
for (global_task_id, ag_id) in self.pcf.events["assigned"][self.pcf.cur_tstep].items():
task_id = global_task_id // self.pcf.max_seq_num
Expand All @@ -2814,7 +2822,9 @@ def move_agents_per_timestep(self) -> None:
self.show_single_task(task_id, seq_id)
self.pcf.event_tracker["aid"] += 1

if self.pcf.cur_tstep == self.pcf.event_tracker["fTime"][self.pcf.event_tracker["fid"]]:
f_times = self.pcf.event_tracker["fTime"]
f_id = self.pcf.event_tracker["fid"]
if f_times and f_id < len(f_times) and self.pcf.cur_tstep == f_times[f_id]:
# from assigned to finished
for (global_task_id, ag_id) in self.pcf.events["finished"][self.pcf.cur_tstep].items():
task_id = global_task_id // self.pcf.max_seq_num
Expand Down Expand Up @@ -2844,8 +2854,10 @@ def back_agents_per_timestep(self) -> None:
# Move the event tracker backward
prev_aid = max(self.pcf.event_tracker["aid"]-1, 0)
prev_fid = max(self.pcf.event_tracker["fid"]-1, 0)
prev_agn_time = self.pcf.event_tracker["aTime"][prev_aid]
prev_fin_time = self.pcf.event_tracker["fTime"][prev_fid]
a_times = self.pcf.event_tracker["aTime"]
f_times = self.pcf.event_tracker["fTime"]
prev_agn_time = a_times[prev_aid] if a_times else -1
prev_fin_time = f_times[prev_fid] if f_times else -1

if self.pcf.cur_tstep == prev_fin_time: # from finished to assigned
for (global_task_id, ag_id) in self.pcf.events["finished"][prev_fin_time].items():
Expand All @@ -2869,7 +2881,7 @@ def back_agents_per_timestep(self) -> None:
self.show_single_task(task_id, seq_id)
self.pcf.event_tracker["aid"] = prev_aid
prev_aid = max(self.pcf.event_tracker["aid"]-1, 0)
prev_agn_time = self.pcf.event_tracker["aTime"][prev_aid]
prev_agn_time = a_times[prev_aid] if a_times else -1

# Compute the previous location
prev_loc:Dict[int, Tuple[int, int]] = {}
Expand Down