| 148 | |
| 149 | |
| 150 | class PathEditor: |
| 151 | def __init__(self, args: Args): |
| 152 | self.args = args |
| 153 | self.map_dir = args.tinynav_map_path |
| 154 | self.paths_json_path = self.map_dir / args.paths_json_name |
| 155 | self.sdf_map_path = self.map_dir / args.sdf_map_name |
| 156 | self.default_sdf_map_path = self.map_dir / args.default_sdf_map_name |
| 157 | |
| 158 | self.occupancy_grid = np.load(self.map_dir / "occupancy_grid.npy") |
| 159 | self.occupancy_meta = np.load(self.map_dir / "occupancy_meta.npy") |
| 160 | self.origin = self.occupancy_meta[:3].astype(np.float32) |
| 161 | self.resolution = float(self.occupancy_meta[3]) |
| 162 | self.shape = tuple(int(v) for v in self.occupancy_grid.shape) |
| 163 | self.current_sdf_map = self._load_sdf_file(self.sdf_map_path) |
| 164 | self.default_sdf_map = self._load_sdf_file(self.default_sdf_map_path) |
| 165 | if self.default_sdf_map is None: |
| 166 | # Before the first replacement there is no separate backup, so the active file is the default map. |
| 167 | self.default_sdf_map = self.current_sdf_map |
| 168 | |
| 169 | self.paths = _load_json(self.paths_json_path) |
| 170 | self.path_id_counter = (max(self.paths.keys()) + 1) if self.paths else 0 |
| 171 | self.selected_path_id: int | None = next(iter(self.paths.keys()), None) |
| 172 | |
| 173 | self.server = viser.ViserServer(host=args.host, port=args.port) |
| 174 | self.server.scene.world_axes.visible = True |
| 175 | self.server.scene.set_up_direction("+z") |
| 176 | |
| 177 | self.path_line_handles: dict[int, viser.SceneHandle] = {} |
| 178 | self.point_handles: dict[tuple[int, int], viser.SceneHandle] = {} |
| 179 | self.gizmo_handles: dict[tuple[int, int], viser.SceneHandle] = {} |
| 180 | self.sdf_preview_handle: viser.SceneHandle | None = None |
| 181 | self.status = None |
| 182 | |
| 183 | def _load_sdf_file(self, path: Path) -> np.ndarray | None: |
| 184 | if not path.exists(): |
| 185 | return None |
| 186 | sdf_map = np.load(path).astype(np.float32) |
| 187 | if sdf_map.shape != self.occupancy_grid.shape: |
| 188 | raise ValueError( |
| 189 | f"{path} shape {sdf_map.shape} does not match occupancy_grid shape {self.occupancy_grid.shape}" |
| 190 | ) |
| 191 | return sdf_map |
| 192 | |
| 193 | def _edited_sdf_map(self) -> np.ndarray: |
| 194 | return build_sdf_from_paths(self.paths, self.origin, self.resolution, self.shape) |
| 195 | |
| 196 | def _ensure_default_backup(self) -> None: |
| 197 | if self.default_sdf_map_path.exists(): |
| 198 | return |
| 199 | if self.current_sdf_map is None: |
| 200 | return |
| 201 | np.save(self.default_sdf_map_path, self.current_sdf_map) |
| 202 | self.default_sdf_map = self.current_sdf_map.copy() |
| 203 | print(f"Saved default SDF backup to {self.default_sdf_map_path}") |
| 204 | |
| 205 | def run(self) -> None: |
| 206 | self._add_static_map_layers() |
| 207 | self._add_optional_pointcloud() |