diff --git a/script/plan_config.py b/script/plan_config.py index a695f08..e538dac 100644 --- a/script/plan_config.py +++ b/script/plan_config.py @@ -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: @@ -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!") @@ -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])) @@ -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): diff --git a/script/plan_viz.py b/script/plan_viz.py index 98422c3..231b18d 100755 --- a/script/plan_viz.py +++ b/script/plan_viz.py @@ -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" @@ -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" @@ -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" @@ -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(): @@ -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(): @@ -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 @@ -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 @@ -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(): @@ -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]] = {}