Spaces:
Running
Running
File size: 2,350 Bytes
ad91e86 | 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 | """Habitat-Sim camera, NavMesh and agent-pose helpers."""
from __future__ import annotations
import math
from pathlib import Path
from typing import Any
import habitat_sim
import numpy as np
def sensor_specs(width: int, height: int) -> list[Any]:
specs = []
for name, sensor_type in (
("rgb", habitat_sim.SensorType.COLOR),
("depth", habitat_sim.SensorType.DEPTH),
("semantic", habitat_sim.SensorType.SEMANTIC),
):
spec = habitat_sim.CameraSensorSpec()
spec.uuid = name
spec.sensor_type = sensor_type
spec.sensor_subtype = habitat_sim.SensorSubType.PINHOLE
spec.resolution = [height, width]
spec.position = [0.0, 1.35, 0.0]
spec.hfov = 79.0
specs.append(spec)
return specs
def make_simulator(
hssd: Path, scene_id: str, navmesh: Path, gpu: int, width: int, height: int
) -> Any:
sim_config = habitat_sim.SimulatorConfiguration()
sim_config.scene_id = str(
(hssd / f"scenes-uncluttered/{scene_id}.scene_instance.json").resolve()
)
sim_config.scene_dataset_config_file = str(
(hssd / "hssd-hab-uncluttered.scene_dataset_config.json").resolve()
)
sim_config.enable_physics = True
sim_config.create_renderer = True
sim_config.load_semantic_mesh = True
sim_config.gpu_device_id = gpu
agent_config = habitat_sim.agent.AgentConfiguration()
agent_config.sensor_specifications = sensor_specs(width, height)
simulator = habitat_sim.Simulator(
habitat_sim.Configuration(sim_config, [agent_config])
)
if not simulator.pathfinder.load_nav_mesh(str(navmesh.resolve())):
simulator.close()
raise RuntimeError(f"cannot load navmesh {navmesh}")
return simulator
def set_agent(agent: Any, position: list[float], xyzw: list[float]) -> None:
state = habitat_sim.AgentState()
state.position = np.asarray(position, dtype=np.float32)
state.rotation = np.asarray(xyzw, dtype=np.float32)
agent.set_state(state)
def path_distance(pathfinder: Any, start: list[float], goal: list[float]) -> float:
path = habitat_sim.ShortestPath()
path.requested_start = np.asarray(start, dtype=np.float32)
path.requested_end = np.asarray(goal, dtype=np.float32)
return float(path.geodesic_distance) if pathfinder.find_path(path) else math.inf
|