diff --git a/.github/workflows/dev-test.yaml b/.github/workflows/dev-test.yaml new file mode 100644 index 0000000..0d34c51 --- /dev/null +++ b/.github/workflows/dev-test.yaml @@ -0,0 +1,46 @@ +name: Dev Branch Test + +on: + workflow_dispatch: + push: + branches: + - dev + +permissions: + contents: read + +jobs: + test: + runs-on: ubuntu-latest + + strategy: + matrix: + python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] + + steps: + - uses: actions/checkout@v4 + + - uses: actions/setup-python@v5 + with: + python-version: ${{ matrix.python-version }} + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + python -m pip install build + python -m pip install . + + - name: Run tests + run: | + for file in tests/*_test.py; do python "$file"; done + + - name: Build distributions + if: matrix.python-version == '3.13' + run: python -m build + + - name: Upload build artifacts + if: matrix.python-version == '3.13' + uses: actions/upload-artifact@v4 + with: + name: dev-dists + path: dist/ \ No newline at end of file diff --git a/.github/workflows/workflow.yaml b/.github/workflows/workflow.yaml index 1fa8189..142620b 100644 --- a/.github/workflows/workflow.yaml +++ b/.github/workflows/workflow.yaml @@ -1,11 +1,3 @@ -# This workflow will upload a Python Package to PyPI when a release is created -# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python#publishing-to-package-registries - -# This workflow uses actions that are not certified by GitHub. -# They are provided by a third-party and are governed by -# separate terms of service, privacy policy, and support -# documentation. - name: Upload Python Package on: @@ -16,23 +8,36 @@ permissions: contents: read jobs: - release-build: + test-and-build: runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] + steps: - uses: actions/checkout@v4 - uses: actions/setup-python@v5 with: - python-version: "3.x" + python-version: ${{ matrix.python-version }} - - name: Build release distributions + - name: Install build tools run: | - # NOTE: put your own distribution build steps here. + python -m pip install --upgrade pip python -m pip install build - python -m build + python -m pip install . + + - name: Run tests + run: | + for file in tests/*_test.py; do python "$file"; done + + - name: Build distributions + if: matrix.python-version == '3.13' + run: python -m build - name: Upload distributions + if: matrix.python-version == '3.13' uses: actions/upload-artifact@v4 with: name: release-dists @@ -40,31 +45,22 @@ jobs: pypi-publish: runs-on: ubuntu-latest - needs: - - release-build + needs: test-and-build + permissions: - # IMPORTANT: this permission is mandatory for trusted publishing id-token: write - # Dedicated environments with protections for publishing are strongly recommended. - # For more information, see: https://docs.github.com/en/actions/deployment/targeting-different-environments/using-environments-for-deployment#deployment-protection-rules environment: name: pypi - # OPTIONAL: uncomment and update to include your PyPI project URL in the deployment status: - # url: https://pypi.org/p/YOURPROJECT - # - # ALTERNATIVE: if your GitHub Release name is the PyPI project version string - # ALTERNATIVE: exactly, uncomment the following line instead: - # url: https://pypi.org/project/YOURPROJECT/${{ github.event.release.name }} steps: - - name: Retrieve release distributions + - name: Download distributions uses: actions/download-artifact@v4 with: name: release-dists path: dist/ - - name: Publish release distributions to PyPI + - name: Publish to PyPI uses: pypa/gh-action-pypi-publish@release/v1 with: - packages-dir: dist/ \ No newline at end of file + packages-dir: dist/ diff --git a/.gitignore b/.gitignore index 93419da..de4763a 100644 --- a/.gitignore +++ b/.gitignore @@ -170,3 +170,8 @@ cython_debug/ # Profile files *.prof + +# OS files +.DS_Store + +.claude/ \ No newline at end of file diff --git a/README.md b/README.md index cddd6a9..41c3811 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# GAMMS v0.2.7 +# GAMMS v1.0.0 GAMMS (Graph based Adversarial Multiagent Modelling Simulator) is a Python library designed for simulating large scale multi-agent scenarios on environments represented as graphs. The library is a framework which focuses on providing a flexible and extensible architecture to facilitate implementing interactions between agents and the environment as well as controlling the information flow between them. The library is geared towards easing development of high level scenario dynamics and testing agent behaviours rather than low level details like the actual physics of real world interactions. GAMMS tries to solve the following problems: @@ -45,4 +45,17 @@ Detailed installation and setup instructions are available in the [Installation # Documentation -The documentation is available at [GAMMS Documentation](https://gammsim.github.io/gamms/stable/). The documentation is generated using [mkdocs-materials](https://squidfunk.github.io/mkdocs-material/) with mike for versioning. You can do a local build by cloning the repository and serving the documentation using mkdocs. For new users, [Tutorials](https://gammsim.github.io/gamms/stable/tutorials) is a good place to start. \ No newline at end of file +The documentation is available at [GAMMS Documentation](https://gammsim.github.io/gamms/stable/). The documentation is generated using [mkdocs-materials](https://squidfunk.github.io/mkdocs-material/) with mike for versioning. You can do a local build by cloning the repository and serving the documentation using mkdocs. For new users, [Tutorials](https://gammsim.github.io/gamms/stable/tutorials) is a good place to start. + +# Citation + +If you use GAMMS in your work, please cite the paper: + + ```bibtex +@article{patil2026gamms, + title={GAMMS: Graph based Adversarial Multiagent Modeling Simulator}, + author={Patil, Rohan and Malegaonkar, Jai and Jiang, Xiao and Dion, Andre and Sukhatme, Gaurav S and Christensen, Henrik I}, + journal={arXiv preprint arXiv:2602.05105}, + year={2026} +} +``` \ No newline at end of file diff --git a/examples/base/config.py b/examples/base/config.py index 1ff2823..55aff81 100644 --- a/examples/base/config.py +++ b/examples/base/config.py @@ -3,10 +3,10 @@ import math # Visualization -vis_engine = gamms.visual.Engine.PYGAME +vis_engine = gamms.visual.Engine.NO_VIS # The path to the graph file -location = "West Point, New York, USA" +location = "La Jolla, CA, USA" resolution = 10.0 graph_path = 'graph.pkl' diff --git a/examples/moving_dot.py b/examples/moving_dot.py new file mode 100644 index 0000000..8b72913 --- /dev/null +++ b/examples/moving_dot.py @@ -0,0 +1,93 @@ +"""Minimal example of a non-agent dynamic artist moving between two fixed points.""" + +import gamms +from gamms.VisualizationEngine import Color, Shape +from gamms.VisualizationEngine.artist import Artist +from gamms.typing import ArtistType, IContext + + +START_POINT = (-8.0, 0.0) +END_POINT = (8.0, 0.0) +VIS_KWARGS = {} + + +def draw_moving_dot(ctx: IContext, data: dict): + start_x, start_y = data["start"] + end_x, end_y = data["end"] + alpha = data.get("_alpha") + + x = (1.0 - alpha) * start_x + alpha * end_x + y = (1.0 - alpha) * start_y + alpha * end_y + + ctx.visual.render_circle( + x, + y, + data.get("radius", 0.9), + data.get("color", Color.Red), + ) + + +def draw_path_line(ctx: IContext, data: dict): + start_x, start_y = data["start"] + end_x, end_y = data["end"] + ctx.visual.render_line( + start_x, + start_y, + end_x, + end_y, + data.get("path_color", Color.LightGray), + width=data.get("width", 2), + ) + + +def add_anchor(ctx: IContext, name: str, x: float, y: float, color): + anchor = Artist(ctx, Shape.Circle, 5) + anchor.data["x"] = x + anchor.data["y"] = y + anchor.data["radius"] = 0.6 + anchor.data["color"] = color + anchor.set_artist_type(ArtistType.STATIC) + ctx.visual.add_artist(name, anchor) + return anchor + + +def add_path_line(ctx: IContext, name: str, start: tuple[float, float], end: tuple[float, float]): + path = Artist(ctx, draw_path_line, 15) + path.data["start"] = start + path.data["end"] = end + path.data["path_color"] = Color.LightGray + path.data["width"] = 2 + path.set_artist_type(ArtistType.STATIC) + ctx.visual.add_artist(name, path) + return path + + +def main() -> None: + ctx = gamms.create_context( + vis_engine=gamms.visual.Engine.PYGAME, + vis_kwargs=VIS_KWARGS, + ) + + add_anchor(ctx, "start_anchor", START_POINT[0], START_POINT[1], Color.Blue) + add_anchor(ctx, "end_anchor", END_POINT[0], END_POINT[1], Color.Green) + add_path_line(ctx, "moving_dot_path", START_POINT, END_POINT) + + moving_dot = Artist(ctx, draw_moving_dot, 20) + moving_dot.set_artist_type(ArtistType.DYNAMIC) + moving_dot.data["start"] = START_POINT + moving_dot.data["end"] = END_POINT + moving_dot.data["radius"] = 0.9 + moving_dot.data["color"] = Color.Red + moving_dot.data["path_color"] = Color.LightGray + ctx.visual.add_artist("moving_dot", moving_dot) + + while not ctx.is_terminated(): + ctx.visual.simulate() + moving_dot.data["start"], moving_dot.data["end"] = moving_dot.data["end"], moving_dot.data["start"] + + ctx.terminate() + + +if __name__ == "__main__": + main() + diff --git a/examples/occlusion_game/ucsd_game.py b/examples/occlusion_game/ucsd_game.py new file mode 100644 index 0000000..8c42551 --- /dev/null +++ b/examples/occlusion_game/ucsd_game.py @@ -0,0 +1,106 @@ +import gamms +from gamms import osm as gamms_osm + +import math + +LOCATION = "University of California San Diego, La Jolla, CA, USA" +RESOLUTION = 10.0 +SENSOR_RANGE = 80.0 +SENSOR_FOV = math.radians(360) # 120° cone + +# --------------------------------------------------------------------------- +# Load graph +# --------------------------------------------------------------------------- +print("Fetching UCSD walk graph from OSM...") +G = gamms_osm.create_osm_graph(LOCATION, gamms_osm.OSMType.WALK, resolution=RESOLUTION) +print(f" {G.number_of_nodes()} nodes, {G.number_of_edges()} edges") + +# --------------------------------------------------------------------------- +# Create context and attach graph +# --------------------------------------------------------------------------- +ctx = gamms.create_context( + graph_engine=gamms.graph.Engine.MEMORY, + vis_engine=gamms.visual.Engine.PYGAME, + vis_kwargs={"width": 1600, "height": 900}, +) +ctx.graph.attach_networkx_graph(G) + +# --------------------------------------------------------------------------- +# Load buildings +# --------------------------------------------------------------------------- +print("Fetching UCSD buildings from OSM (may take ~30s)...") +face_count = 0 +for face in gamms_osm.obstacle_from_osm(LOCATION): + ctx.graph.add_obstacle_face( + face["face_id"], + tr=face["tr"], + tl=face["tl"], + br=face["br"], + bl=face["bl"], + type=face["type"], + ) + face_count += 1 +print(f" {face_count} building wall faces loaded") + +# --------------------------------------------------------------------------- +# Sensors + agent +# --------------------------------------------------------------------------- +start_node = next(ctx.graph.graph.get_nodes()) + +ctx.sensor.create_sensor("neighbor", gamms.sensor.SensorType.NEIGHBOR) +ctx.sensor.create_sensor( + "arc", + gamms.sensor.SensorType.ARC, + sensor_range=SENSOR_RANGE, + fov=SENSOR_FOV, +) +ctx.sensor.create_sensor( + "occ_map", + gamms.sensor.SensorType.OCCLUDED_MAP, + sensor_range=SENSOR_RANGE, + fov=SENSOR_FOV, +) + +ctx.agent.create_agent("player", sensors=["neighbor", "arc", "occ_map"], start_node_id=start_node) +ctx.sensor.get_sensor("arc").set_owner("player") +ctx.sensor.get_sensor("occ_map").set_owner("player") + +# --------------------------------------------------------------------------- +# Visuals +# --------------------------------------------------------------------------- +ctx.visual.set_graph_visual() +ctx.visual.set_obstacle_visual() +ctx.visual.set_agent_visual("player", color="blue", size=10) +ctx.visual.set_sensor_visual("arc", node_color=(255, 200, 0), edge_color=(200, 160, 0)) +ctx.visual.set_sensor_visual("occ_map", node_color=(0, 220, 220), edge_color=(0, 180, 180)) + +# --------------------------------------------------------------------------- +# Game loop +# --------------------------------------------------------------------------- +print("\n=== Game started ===\n") + +turn = 0 +while not ctx.is_terminated(): + agent = ctx.agent.get_agent("player") + state = agent.get_state() + + arc_data = state["sensor"]["arc"][1] + occ = state["sensor"]["occ_map"][1] + arc_nodes = arc_data.get("nodes", {}) + vis_nodes = occ.get("nodes", {}) + neighbors = state["sensor"]["neighbor"][1] + hidden = len(arc_nodes) - len(vis_nodes) + + turn += 1 + print(f"Turn {turn:>3} | node {agent.current_node_id:>6} " + f"| arc: {len(arc_nodes):>4} occluded: {len(vis_nodes):>4} hidden by buildings: {hidden:>4} " + f"| neighbors: {sorted(neighbors)}") + + next_node = ctx.visual.human_input("player", state) + state["action"] = next_node + agent.set_state() + + ctx.visual.simulate() + +ctx.terminate() +print("Game over.") diff --git a/gamms/GraphEngine/graph_engine.py b/gamms/GraphEngine/graph_engine.py index f251486..aab38b2 100644 --- a/gamms/GraphEngine/graph_engine.py +++ b/gamms/GraphEngine/graph_engine.py @@ -1,71 +1,65 @@ import networkx as nx -from typing import Dict, Any, Iterator, cast, Union, Set, overload +from typing import Dict, Any, Iterator, Mapping, Tuple, cast, Union, Set, overload from enum import Enum -from gamms.typing import Node, OSMEdge, IGraph, IGraphEngine, IContext +from gamms.typing import Node, OSMEdge, IGraph, IGraphEngine, IContext, ObsFace from gamms.typing.graph_engine import Engine -import pickle +from gamms.typing.memory_engine import StoreType +from gamms.MemoryEngine.memory_engine import MemoryStore, SqliteStore, PathLike from shapely.geometry import LineString from dataclasses import dataclass -import sqlite3 - import tempfile -import cbor2 - -_mem_Node = dataclass()(Node) -_mem_OSMEdge = dataclass()(OSMEdge) +_Node = dataclass()(Node) +_OSMEdge = dataclass()(OSMEdge) +_ObsFace = dataclass()(ObsFace) class Graph(IGraph): - def __init__(self): - self.nodes: Dict[int, Node] = {} - self.edges: Dict[int, OSMEdge] = {} + def __init__(self, store: MemoryStore): + self.store = store + self.store.create_map( + "nodes", + primary_key="id", + schema={"id": int, "x": float, "y": float} + ) + self.store.create_map( + "edges", + primary_key="id", + schema={"id": int, "source": int, "target": int, "length": float, "linestring": LineString} + ) self._adjacency: Dict[int, Set[int]] = {} - + def get_edge(self, edge_id: int) -> OSMEdge: - return self.edges[edge_id] + return _OSMEdge(**self.store.get_data("edges", edge_id)) @overload def get_edges(self) -> Iterator[int]: ... @overload def get_edges(self, d: float, x: float, y: float) -> Iterator[int]: ... def get_edges(self, d: float = -1.0, x: float = 0, y: float = 0) -> Iterator[int]: - return iter(self.edges.keys()) + return iter(self.store.query_keys("edges")) def get_node(self, node_id: int) -> Node: - return self.nodes[node_id] + return _Node(**self.store.get_data("nodes", node_id)) @overload def get_nodes(self) -> Iterator[int]: ... @overload def get_nodes(self, d: float, x: float, y: float) -> Iterator[int]: ... def get_nodes(self, d: float = -1.0, x: float = 0, y: float = 0) -> Iterator[int]: - return iter(self.nodes.keys()) - - def add_node(self, node_data: Dict[str, Any]) -> None: - if 'id' not in node_data or 'x' not in node_data or 'y' not in node_data: - raise ValueError("Node data must include 'id', 'x', and 'y'.") + return iter(self.store.query_keys("nodes")) - if node_data['id'] in self.nodes: - raise KeyError(f"Node {node_data['id']} already exists.") - - node = _mem_Node(id=node_data['id'], x=node_data['x'], y=node_data['y']) - self.nodes[node_data['id']] = node + def add_node(self, node_data: Dict[str, Any]) -> None: + self.store.insert_data("nodes", node_data) self._adjacency[node_data['id']] = set() def add_edge(self, edge_data: Dict[str, Any]) -> None: - if 'id' not in edge_data or 'source' not in edge_data or 'target' not in edge_data or 'length' not in edge_data: - raise ValueError("Edge data must include 'id', 'source', 'target', and 'length'.") - - if edge_data['id'] in self.edges: - raise KeyError(f"Edge {edge_data['id']} already exists.") - linestring = edge_data.get('linestring', None) + source_node = self.get_node(edge_data['source']) + target_node = self.get_node(edge_data['target']) if linestring is None: # Create a LineString from the source and target node coordinates - source_node = self.get_node(edge_data['source']) - target_node = self.get_node(edge_data['target']) linestring = LineString([(source_node.x, source_node.y), (target_node.x, target_node.y)]) elif not isinstance(linestring, LineString): try: @@ -74,63 +68,42 @@ def add_edge(self, edge_data: Dict[str, Any]) -> None: raise ValueError(f"Invalid linestring data: {linestring}") from e if linestring.is_empty: raise ValueError(f"Invalid linestring: {linestring}") - - if edge_data['source'] not in self.nodes or edge_data['target'] not in self.nodes: - raise KeyError(f"Source or target node does not exist in the graph: {edge_data['source']}, {edge_data['target']}") - - edge = _mem_OSMEdge( - id = edge_data['id'], - source=edge_data['source'], - target=edge_data['target'], - length=edge_data['length'], - linestring=linestring - ) - self.edges[edge_data['id']] = edge + edge_data['linestring'] = linestring + + self.store.insert_data("edges", edge_data) self._adjacency[edge_data['source']].add(edge_data['target']) def update_node(self, node_data: Dict[str, Any]) -> None: - - if node_data['id'] not in self.nodes: - raise KeyError(f"Node {node_data['id']} does not exist.") - - node = self.nodes[node_data['id']] - node.x = node_data.get('x', node.x) - node.y = node_data.get('y', node.y) + self.store.update_data("nodes", node_data) def update_edge(self, edge_data: Dict[str, Any]) -> None: - - if edge_data['id'] not in self.edges: - raise KeyError(f"Edge {edge_data['id']} does not exist. Use add_edge to create it.") - edge = self.edges[edge_data['id']] - - self._adjacency[edge.source].discard(edge.target) - - edge.source = edge_data.get('source', edge.source) - edge.target = edge_data.get('target', edge.target) - edge.length = edge_data.get('length', edge.length) - edge.linestring = edge_data.get('linestring', edge.linestring) - - self._adjacency[edge.source].add(edge.target) + existing_edge = self.get_edge(edge_data['id']) + self._adjacency[existing_edge.source].discard(existing_edge.target) + self.store.update_data("edges", edge_data) + self._adjacency[edge_data['source']].add(edge_data['target']) def remove_node(self, node_id: int) -> None: - if node_id not in self.nodes: + if node_id not in self._adjacency: return - edges_to_remove = [key for key, edge in self.edges.items() if edge.source == node_id or edge.target == node_id] + edges_to_remove = [] + for edge_id in self.get_edges(): + edge = self.get_edge(edge_id) + if edge.source == node_id or edge.target == node_id: + edges_to_remove.append(edge_id) for key in edges_to_remove: - del self.edges[key] - del self.nodes[node_id] + self.store.delete_data("edges", key) + self.store.delete_data("nodes", node_id) + del self._adjacency[node_id] for neighbors in self._adjacency.values(): neighbors.discard(node_id) def remove_edge(self, edge_id: int) -> None: - if edge_id not in self.edges: - return - edge = self.edges[edge_id] + edge = self.get_edge(edge_id) self._adjacency[edge.source].discard(edge.target) - del self.edges[edge_id] + self.store.delete_data("edges", edge_id) def attach_networkx_graph(self, G: nx.Graph) -> None: for node, data in G.nodes(data=True): # type: ignore @@ -171,144 +144,87 @@ def attach_networkx_graph(self, G: nx.Graph) -> None: def get_neighbors(self, node_id: int) -> Iterator[int]: - if node_id not in self.nodes: + if node_id not in self._adjacency: raise KeyError(f"Node {node_id} does not exist.") for neighbor in self._adjacency[node_id]: yield neighbor - - def save(self, path: str) -> None: - """ - Saves the graph to a file. - """ - pickle.dump({"nodes": self.nodes, "edges": self.edges}, open(path, 'wb')) - print(f"Graph saved to {path}") - - def load(self, path: str) -> None: - """ - Loads the graph from a file. - """ - data = pickle.load(open(path, 'rb')) - self.nodes = data['nodes'] - self.edges = data['edges'] - self._adjacency = {node_id: set() for node_id in self.nodes.keys()} - for edge in self.edges.values(): - self._adjacency[edge.source].add(edge.target) - -_sql_Node = _mem_Node - -class _sql_OSMEdge(OSMEdge): - __slots__ = ('id', 'source', 'target', 'length', '_geom') - - def __init__(self, row: sqlite3.Row): - self.id: int = row[0] - self.source: int = row[1] - self.target: int = row[2] - self.length: float = row[3] - self._geom = row[4] - - @property - def linestring(self) -> LineString: - return LineString(cbor2.loads(self._geom)) class SqliteGraph(IGraph): - def __init__(self): - # Create a random name for the SQLite database - self._dbdir = tempfile.TemporaryDirectory(dir=".") - self._conn = sqlite3.connect(f"{self._dbdir.name}/graph.db", isolation_level=None) - self._cursor = self._conn.cursor() + def __init__(self, store: SqliteStore): + self.store = store + # Enable foreign key constraints and set journal mode to WAL for better concurrency # Also set temp_store to MEMORY for faster temporary storage - self._cursor.executescript( + self.store.connection().executescript( "PRAGMA foreign_keys = ON; PRAGMA journal_mode = WAL;PRAGMA temp_store = MEMORY;" + ) + + self.store.create_map( + "nodes", + primary_key="id", + schema={"id": int, "x": float, "y": float} ) - self._cursor.execute( - "CREATE TABLE IF NOT EXISTS nodes (id INTEGER PRIMARY KEY, x REAL, y REAL)" + + self.store.create_map( + "edges", + primary_key="id", + schema={"id": int, "source": int, "target": int, "length": float, "linestring": LineString} ) - # Create index on node x,y for faster lookups - self._cursor.execute("CREATE INDEX IF NOT EXISTS idx_nodes_xy ON nodes (x, y)") - self._cursor.execute( - "CREATE TABLE IF NOT EXISTS edges (id INTEGER PRIMARY KEY, source INTEGER, target INTEGER, length REAL, geom BLOB, FOREIGN KEY(source) REFERENCES nodes(id), FOREIGN KEY(target) REFERENCES nodes(id))" + + self.store.connection().execute("CREATE INDEX IF NOT EXISTS idx_nodes_xy ON nodes (x, y)") + self.store.connection().executescript( + """ + ALTER TABLE edges RENAME TO old_edges; + CREATE TABLE edges ( + id INTEGER PRIMARY KEY, + source INTEGER NOT NULL, + target INTEGER NOT NULL, + length REAL NOT NULL, + linestring BLOB, + FOREIGN KEY (source) REFERENCES nodes(id) ON DELETE CASCADE, + FOREIGN KEY (target) REFERENCES nodes(id) ON DELETE CASCADE + ); + + DROP TABLE old_edges; + """ ) - # Create index on edge source,target for faster lookups - self._cursor.execute("CREATE INDEX IF NOT EXISTS idx_edges_source_target ON edges (source, target)") - self._conn.commit() - self._call_commit = False - - def __del__(self): - """ - Destructor to close the database connection. - """ - self._conn.close() - if self._dbdir: - self._dbdir.cleanup() + self.store.connection().execute("CREATE INDEX IF NOT EXISTS idx_edges_source_target ON edges (source, target)") def add_node(self, node_data: Dict[str, Any]) -> None: """ Adds a node to the graph. """ - if 'id' not in node_data or 'x' not in node_data or 'y' not in node_data: - raise ValueError("Node data must include 'id', 'x', and 'y'.") - - try: - self._cursor.execute("INSERT INTO nodes (id, x, y) VALUES (?, ?, ?)", - (node_data['id'], node_data['x'], node_data['y'])) - except sqlite3.IntegrityError as e: - if "UNIQUE constraint failed" in str(e): - raise KeyError(f"Node {node_data['id']} already exists.") from e - - self._call_commit = True + self.store.insert_data("nodes", node_data) + def add_edge(self, edge_data: Dict[str, Any]) -> None: """ Adds an edge to the graph. """ - if 'id' not in edge_data or 'source' not in edge_data or 'target' not in edge_data or 'length' not in edge_data: - raise ValueError("Edge data must include 'id', 'source', 'target', and 'length'.") - linestring = edge_data.get('linestring', None) + source_node = self.get_node(edge_data['source']) + target_node = self.get_node(edge_data['target']) if linestring is None: # Create a LineString from the source and target node coordinates - com_val = self._call_commit - self._call_commit = False - source_node = self.get_node(edge_data['source']) - target_node = self.get_node(edge_data['target']) - self._call_commit = com_val - linestring = ((source_node.x, source_node.y), (target_node.x, target_node.y)) + linestring = LineString([(source_node.x, source_node.y), (target_node.x, target_node.y)]) elif not isinstance(linestring, LineString): try: linestring = LineString(linestring) - linestring = tuple(linestring.coords) except Exception as e: raise ValueError(f"Invalid linestring data: {linestring}") from e - else: - linestring = tuple(linestring.coords) + if linestring.is_empty: + raise ValueError(f"Invalid linestring: {linestring}") - try: - self._cursor.execute("INSERT INTO edges (id, source, target, length, geom) VALUES (?, ?, ?, ?, ?)", - (edge_data['id'], edge_data['source'], edge_data['target'], edge_data['length'], cbor2.dumps(linestring))) - except sqlite3.IntegrityError as e: - if "UNIQUE constraint failed" in str(e): - raise KeyError(f"Edge {edge_data['id']} already exists.") from e - elif "FOREIGN KEY constraint failed" in str(e): - raise KeyError(f"Source or target node does not exist in the graph: {edge_data['source']}, {edge_data['target']}") from e + edge_data['linestring'] = tuple(linestring.coords) - self._call_commit = True + self.store.insert_data("edges", edge_data) def get_node(self, node_id: int) -> Node: """ Retrieves a node by its ID. """ - if self._call_commit: - self._conn.commit() - self._call_commit = False - cursor = self._conn.cursor() - cursor.execute("SELECT id, x, y FROM nodes WHERE id = ?", (node_id,)) - row = cursor.fetchone() - if row is None: - raise KeyError(f"Node {node_id} does not exist.") - - return _sql_Node(id=row[0], x=row[1], y=row[2]) + return _Node(**self.store.get_data("nodes", node_id)) @overload def get_edges(self) -> Iterator[int]: ... @@ -318,10 +234,8 @@ def get_edges(self, d: float = -1.0, x: float = 0, y: float = 0) -> Iterator[int """ Returns an iterator over all edge IDs in the graph. """ - if self._call_commit: - self._conn.commit() - self._call_commit = False - cursor = self._conn.cursor() + self.store.flush() # Ensure all pending changes are written to the database + cursor = self.store.connection().cursor() if d >= 0: x_min, x_max = x - d, x + d y_min, y_max = y - d, y + d @@ -339,16 +253,7 @@ def get_edge(self, edge_id: int) -> OSMEdge: """ Retrieves an edge by its ID. """ - if self._call_commit: - self._conn.commit() - self._call_commit = False - cursor = self._conn.cursor() - cursor.execute("SELECT id, source, target, length, geom FROM edges WHERE id = ?", (edge_id,)) - row = cursor.fetchone() - if row is None: - raise KeyError(f"Edge {edge_id} does not exist.") - - return _sql_OSMEdge(row) + return _OSMEdge(**self.store.get_data("edges", edge_id)) @overload def get_nodes(self) -> Iterator[int]: ... @@ -358,10 +263,8 @@ def get_nodes(self, d: float = -1.0, x: float = 0, y: float = 0) -> Iterator[int """ Returns an iterator over all node IDs in the graph. """ - if self._call_commit: - self._conn.commit() - self._call_commit = False - cursor = self._conn.cursor() + self.store.flush() + cursor = self.store.cursor() if d >= 0: cursor.execute("SELECT id FROM nodes WHERE x BETWEEN ? AND ? AND y BETWEEN ? AND ?", (x - d, x + d, y - d, y + d)) else: @@ -376,102 +279,44 @@ def update_node(self, node_data: Dict[str, Any]) -> None: """ Updates a node in the graph. """ - if 'id' not in node_data or 'x' not in node_data or 'y' not in node_data: - raise ValueError("Node data must include 'id', 'x', and 'y'.") - - _ = self.get_node(node_data['id']) - - self._cursor.execute("UPDATE nodes SET x = ?, y = ? WHERE id = ?", - (node_data['x'], node_data['y'], node_data['id'])) - - self._call_commit = True + self.store.update_data("nodes", node_data) def update_edge(self, edge_data: Dict[str, Any]) -> None: """ Updates an edge in the graph. """ - if 'id' not in edge_data or 'source' not in edge_data or 'target' not in edge_data or 'length' not in edge_data: - raise ValueError("Edge data must include 'id', 'source', 'target', and 'length'.") - - _ = self.get_edge(edge_data['id']) - linestring = edge_data.get('linestring', None) - if linestring is None: - # Create a LineString from the source and target node coordinates - source_node = self.get_node(edge_data['source']) - target_node = self.get_node(edge_data['target']) - linestring = ((source_node.x, source_node.y), (target_node.x, target_node.y)) - elif not isinstance(linestring, LineString): - try: - linestring = LineString(linestring) - linestring = tuple(linestring.coords) - except Exception as e: - raise ValueError(f"Invalid linestring data: {linestring}") from e - else: - linestring = tuple(linestring.coords) - - self._cursor.execute("UPDATE edges SET source = ?, target = ?, length = ?, geom = ? WHERE id = ?", - (edge_data['source'], edge_data['target'], edge_data['length'], cbor2.dumps(linestring), edge_data['id'])) - - self._call_commit = True + if linestring is not None: + edge_data['linestring'] = tuple(LineString(linestring).coords) + self.store.update_data("edges", edge_data) def remove_node(self, node_id: int) -> None: """ Removes a node from the graph. """ - # Remove edges associated with this node - self._cursor.execute("DELETE FROM edges WHERE source = ? OR target = ?", (node_id, node_id)) - self._cursor.execute("DELETE FROM nodes WHERE id = ?", (node_id,)) - self._call_commit = True - + try: + self.store.delete_data("nodes", node_id) + except KeyError: + pass # Node does not exist, ignore + def remove_edge(self, edge_id: int) -> None: """ Removes an edge from the graph. """ - self._cursor.execute("DELETE FROM edges WHERE id = ?", (edge_id,)) - - self._call_commit = True - - def attach_networkx_graph(self, G: nx.Graph) -> None: - """ - Attaches a NetworkX graph to the SqliteGraph object. - """ - for node, data in G.nodes(data=True): # type: ignore - node = cast(int, node) - data = cast(Dict[str, Any], data) - node_data: Dict[str, Union[int, float]] = { - 'id': node, - 'x': data.get('x', 0.0), - 'y': data.get('y', 0.0) - } - self.add_node(node_data) - - self._conn.commit() # Commit after adding all nodes to ensure they are available for edge insertion - self._call_commit = False # Reset call_commit flag after manual commit - - for u, v, data in G.edges(data=True): # type: ignore - u = cast(int, u) - v = cast(int, v) - data = cast(Dict[str, Any], data) - linestring = data.get('linestring', None) - edge_data: Dict[str, Any] = { - 'id': data.get('id', -1), - 'source': u, - 'target': v, - 'length': data.get('length', 0.0), - 'linestring': linestring - } - self.add_edge(edge_data) - - self._conn.commit() # Commit after adding all edges - self._call_commit = False # Reset call_commit flag after manual commit + try: + self.store.delete_data("edges", edge_id) + except KeyError: + pass # Edge does not exist, ignore + + attach_networkx_graph = Graph.attach_networkx_graph + def get_neighbors(self, node_id: int) -> Iterator[int]: """ Returns an iterator over the neighbors of a given node. """ _ = self.get_node(node_id) - cursor = self._conn.cursor() + cursor = self.store.connection().cursor() cursor.execute("SELECT target FROM edges WHERE source = ?", (node_id,)) while True: row = cursor.fetchone() @@ -482,9 +327,42 @@ def get_neighbors(self, node_id: int) -> Iterator[int]: class GraphEngine(IGraphEngine): def __init__(self, ctx: IContext, engine: Enum = Engine.SQLITE): if engine == Engine.MEMORY: - self._graph = Graph() + self._store = ctx.ictx.memory.create_store(StoreType.MEMORY, name="graph_store") + self._store = cast(MemoryStore, self._store) + self._graph = Graph(self._store) + self._store.create_map( + "obstacle_face", + primary_key="id", + schema={ + "id": int, + "trx": float, "try": float, "trz": float, + "brx": float, "bry": float, "brz": float, + "tlx": float, "tly": float, "tlz": float, + "blx": float, "bly": float, "blz": float, + "type": int + } + ) elif engine == Engine.SQLITE: - self._graph = SqliteGraph() + self._dbdir = tempfile.TemporaryDirectory(dir=".") + path = PathLike(f"{self._dbdir.name}/graph.db") + self._store = ctx.ictx.memory.create_store(StoreType.DATABASE, name="graph_store", path=path) + self._store = cast(SqliteStore, self._store) + self._graph = SqliteGraph(self._store) + self._store.create_map( + "obstacle_face", + primary_key="id", + schema={ + "id": int, + "trx": float, "try": float, "trz": float, + "brx": float, "bry": float, "brz": float, + "tlx": float, "tly": float, "tlz": float, + "blx": float, "bly": float, "blz": float, + "type": int + } + ) + self._store.connection().execute( + "CREATE INDEX IF NOT EXISTS idx_obstacle_face ON obstacle_face (trx, try, brx, bry, tlx, tly, blx, bly)" + ) else: raise ValueError(f"Unsupported engine type: {engine}") self.ctx = ctx @@ -493,6 +371,80 @@ def __init__(self, ctx: IContext, engine: Enum = Engine.SQLITE): def graph(self) -> IGraph: return self._graph + def add_obstacle_face( + self, + face_id: int, + tr: Tuple[float, float, float], + tl: Tuple[float, float, float], + br: Tuple[float, float, float], + bl: Tuple[float, float, float], + type: int + ) -> None: + try: + self._store.insert_data("obstacle_face", { + "id": face_id, + "trx": tr[0], "try": tr[1], "trz": tr[2], + "brx": br[0], "bry": br[1], "brz": br[2], + "tlx": tl[0], "tly": tl[1], "tlz": tl[2], + "blx": bl[0], "bly": bl[1], "blz": bl[2], + "type": type + }) + except ValueError as e: + raise ValueError(f"Failed to add obstacle face with ID {face_id}: {e}") from e + except KeyError: + raise KeyError(f"Obstacle face with ID {face_id} already exists.") + except Exception as e: + raise ValueError(f"Unexpected error occurred while adding obstacle face with ID {face_id}.") from e + + def remove_obstacle_face(self, face_id: int) -> None: + self._store.delete_data("obstacle_face", face_id) + + def get_obstacle_face(self, face_id: int) -> ObsFace: + ret = self._store.get_data("obstacle_face", face_id) + ret = { + 'id': ret['id'], + 'tr': (ret['trx'], ret['try'], ret['trz']), + 'br': (ret['brx'], ret['bry'], ret['brz']), + 'tl': (ret['tlx'], ret['tly'], ret['tlz']), + 'bl': (ret['blx'], ret['bly'], ret['blz']), + 'type': ret['type'] + } + return _ObsFace(**ret) + + def get_obstacle_faces(self, d: float = -1.0, x: float = 0, y: float = 0) -> Iterator[int]: + if self._store.type == StoreType.MEMORY: + for key in self._store.query_keys("obstacle_face"): + if d >= 0: + data = self._store.get_data("obstacle_face", key) + if not (max(data['trx'], data['tlx'], data['brx'], data['blx']) >= x - d and + min(data['trx'], data['tlx'], data['brx'], data['blx']) <= x + d and + max(data['try'], data['tly'], data['bry'], data['bly']) >= y - d and + min(data['try'], data['tly'], data['bry'], data['bly']) <= y + d): + continue + yield key + elif self._store.type == StoreType.DATABASE: + cursor = self._store.connection().cursor() + if d >= 0: + cursor.execute( + """SELECT id FROM obstacle_face WHERE ( + MAX(tlx, trx, blx, brx) >= ? + AND MIN(tlx, trx, blx, brx) <= ? + AND MAX(tly, try, bly, bry) >= ? + AND MIN(tly, try, bly, bry) <= ? + )""", + (x - d, x + d, y - d, y + d) + ) + else: + cursor.execute("SELECT id FROM obstacle_face") + while True: + row = cursor.fetchone() + if row is None: + break + yield row[0] + else: + raise ValueError(f"Unsupported store type: {self._store.type}") + + def attach_networkx_graph(self, G: nx.Graph) -> IGraph: """ Attaches a NetworkX graph to the Graph object. @@ -511,6 +463,8 @@ def load(self, path: str) -> IGraph: return self.graph def terminate(self): + if self._store.type == StoreType.DATABASE: + self._dbdir.cleanup() try: del self._graph except Exception as e: diff --git a/gamms/MemoryEngine/__init__.py b/gamms/MemoryEngine/__init__.py new file mode 100644 index 0000000..dc86f02 --- /dev/null +++ b/gamms/MemoryEngine/__init__.py @@ -0,0 +1,3 @@ +from gamms.MemoryEngine.memory_engine import MemoryEngine, MemoryStore, SqliteStore, PathLike + +__all__ = ["MemoryEngine", "MemoryStore", "SqliteStore", "PathLike"] diff --git a/gamms/MemoryEngine/memory_engine.py b/gamms/MemoryEngine/memory_engine.py index 6377fd5..b2bd8b0 100644 --- a/gamms/MemoryEngine/memory_engine.py +++ b/gamms/MemoryEngine/memory_engine.py @@ -1,30 +1,58 @@ -from gamms.typing.memory_engine import IMemoryEngine, StoreType -from gamms.MemoryEngine.store import Store, PathLike -import os -from typing import Any, List +from typing import Dict, Iterator, Optional + +from gamms.typing.memory_engine import IMemoryEngine, IPathLike, IStore, StoreType +from gamms.MemoryEngine.store import MemoryStore, PathLike, SqliteStore + class MemoryEngine(IMemoryEngine): + """Default :class:`IMemoryEngine` implementation. + + Owns a registry of stores, each of which carries its own backend. + """ + def __init__(self) -> None: - self.stores = {} - - def create_store(self, store_type: StoreType, name: str, path: PathLike, obj: Any) -> 'Store': - if name in self.stores: - raise ValueError(f"Store with name '{name}' already exists.") - - new_store = Store(name, store_type, path) - new_store.save(obj) - self.stores[name] = new_store - return new_store - - - def list_store(self) -> List[str]: - return list(self.stores.keys) - - def load_store(self, name: str) -> Any: - if name not in self.stores: - raise ValueError(f"Store with name '{name}' does not exist.") - - return self.stores[name].load() - - def terminate(self): - return \ No newline at end of file + self._stores: Dict[str, IStore] = {} + + @staticmethod + def _build_store( + store_type: StoreType, + name: str, + path: Optional[IPathLike], + ) -> IStore: + if store_type == StoreType.MEMORY: + return MemoryStore(name, path) # type: ignore[arg-type] + if store_type == StoreType.DATABASE: + if path is None: + raise ValueError("DATABASE store requires a path.") + return SqliteStore(name, path) # type: ignore[arg-type] + if store_type == StoreType.FILESYSTEM: + raise NotImplementedError("FILESYSTEM store type is not implemented yet.") + raise ValueError(f"Unsupported store type: {store_type}") + + def create_store( + self, + store_type: StoreType, + name: str, + path: Optional[IPathLike] = None, + ) -> IStore: + if name in self._stores: + raise ValueError(f"Store with name {name!r} already exists.") + store = self._build_store(store_type, name, path) + self._stores[name] = store + return store + + def get_store(self, name: str) -> IStore: + if name not in self._stores: + raise KeyError(f"Store with name {name!r} does not exist.") + return self._stores[name] + + def list_stores(self) -> Iterator[str]: + return iter(self._stores.keys()) + + def terminate(self) -> None: + for store in self._stores.values(): + store.close() + self._stores.clear() + + +__all__ = ["MemoryEngine", "MemoryStore", "SqliteStore", "PathLike"] diff --git a/gamms/MemoryEngine/store.py b/gamms/MemoryEngine/store.py index 6dab253..855cb07 100644 --- a/gamms/MemoryEngine/store.py +++ b/gamms/MemoryEngine/store.py @@ -1,54 +1,351 @@ -from gamms.typing.memory_engine import IStore, StoreType, IPathLike import os -from typing import Any +import sqlite3 +from typing import Any, Dict, Iterator, List, Mapping, Optional, Tuple, Type + +import cbor2 + +from gamms.typing.memory_engine import IPathLike, IStore, StoreType + + +_PRIMITIVE_TYPES = (int, float, str, bool, bytes) + class PathLike(IPathLike): + """Local filesystem resource locator. + + ``IPathLike`` is intentionally generic so future implementations can + locate remote resources (carrying scheme/host/port). This concrete class + is the only one we ship today and treats ``path`` as an opaque string. + """ + def __init__(self, path: str): if not path: raise ValueError("Path cannot be empty.") - self.path = os.path.abspath(path) - + self.path = path + def exists(self) -> bool: return os.path.exists(self.path) def as_str(self) -> str: return self.path -class Store(IStore): - def __init__(self, name: str, store_type: StoreType, path: PathLike): - self.name = name - self.store_type = store_type - self.path = path + def __repr__(self) -> str: + return f"PathLike({self.path!r})" + + +def _validate_struct(struct: Dict[str, Type], primary_key: str) -> None: + if not isinstance(struct, dict) or not struct: + raise TypeError("struct must be a non-empty dict mapping field names to types.") + for field, field_type in struct.items(): + if not isinstance(field, str) or not field: + raise TypeError(f"Field name must be a non-empty string, got {field!r}.") + if not isinstance(field_type, type): + raise TypeError(f"Field {field!r} type must be a Python type, got {field_type!r}.") + + if primary_key not in struct: + raise IndexError(f"Primary key {primary_key!r} not present in struct.") + # Primary keys must be hashable and indexable; disallow unhashable collection types and non-scalar primitives. + if struct[primary_key] not in _PRIMITIVE_TYPES: + raise IndexError( + f"Primary key {primary_key!r} must be an indexable scalar type, " + f"got {struct[primary_key].__name__!r}." + ) + +class MemoryStore(IStore): + def __init__(self, name: str, path: Optional[PathLike] = None): + self._name = name + self._path = path + self._maps: Dict[str, Dict[Any, Dict[str, Any]]] = {} + self._schemas: Dict[str, Tuple[Dict[str, Type], str]] = {} + + def name(self) -> str: + return self._name + + def path(self) -> Optional[IPathLike]: + return self._path + + @property + def type(self) -> StoreType: + return StoreType.MEMORY + + def create_map(self, map_name: str, schema: Dict[str, Type], primary_key: str) -> None: + if map_name in self._maps: + raise ValueError(f"Map {map_name!r} already exists in store {self._name!r}.") + _validate_struct(schema, primary_key) + self._maps[map_name] = {} + self._schemas[map_name] = (schema, primary_key) + + def delete_map(self, map_name: str) -> None: + if map_name not in self._maps: + raise KeyError(f"Map {map_name!r} does not exist in store {self._name!r}.") + del self._maps[map_name] + del self._schemas[map_name] + + def list_maps(self) -> List[str]: + return list(self._maps.keys()) + + def _require_map(self, map_name: str) -> Tuple[Dict[Any, Dict[str, Any]], Dict[str, Type], str]: + if map_name not in self._maps: + raise IndexError(f"Map {map_name!r} does not exist in store {self._name!r}.") + schema, pk = self._schemas[map_name] + return self._maps[map_name], schema, pk + + def insert_data(self, map_name: str, struct: Dict[str, Any]) -> None: + rows, schema, pk = self._require_map(map_name) + for field in schema: + if field not in struct: + raise ValueError(f"Field {field!r} not declared in map schema.") + if pk not in struct: + raise ValueError(f"Primary key {pk!r} missing from struct.") + key = struct[pk] + if key in rows: + raise KeyError(f"Key {key!r} already exists in map {map_name!r}.") + rows[key] = struct + + def get_data(self, map_name: str, key: Any) -> Mapping[str, Any]: + rows, schema, _ = self._require_map(map_name) + if key not in rows: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + row = rows[key] + return row + + def update_data(self, map_name: str, struct: Dict[str, Any]) -> None: + rows, _, pk = self._require_map(map_name) + if pk not in struct: + raise ValueError(f"Primary key {pk!r} missing from struct.") + key = struct[pk] + if key not in rows: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + rows[key].update(struct) + + def delete_data(self, map_name: str, key: Any) -> None: + rows, _, _ = self._require_map(map_name) + if key not in rows: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + del rows[key] + + def query_keys(self, map_name: str) -> Iterator[Any]: + rows, _, _ = self._require_map(map_name) + return iter(rows.keys()) + + def close(self) -> None: + self._maps.clear() + self._schemas.clear() + + - def save(self, obj: Any) -> None: - if self.store_type == StoreType.FILESYSTEM: - os.makedirs(self.path.as_str(), exist_ok=True) - file_path = os.path.join(self.path.as_str(), self.name) - with open(file_path, 'w') as file: - if isinstance(obj, str) and os.path.exists(obj): - with open(obj, 'r') as object_file: - file.write(object_file.read()) - else: - file.write(str(obj)) - else: - raise NotImplementedError(f"Save operation not implemented for {self.store_type}.") - - def load(self) -> Any: - if self.store_type == StoreType.FILESYSTEM: - file_path = os.path.join(self.path.as_str(), self.name) - if not os.path.exists(file_path): - raise FileNotFoundError(f"Store file '{file_path}' does not exist.") - with open(file_path, 'r') as file: - return file.read() - else: - raise NotImplementedError(f"Load operation not implemented for {self.store_type}.") - - def delete(self) -> None: - if self.store_type == StoreType.FILESYSTEM: - file_path = os.path.join(self.path.as_str(), self.name) - if os.path.exists(file_path): - os.remove(file_path) +_PY_TO_SQL = { + int: "INTEGER", + float: "REAL", + str: "TEXT", + bool: "INTEGER", +} + + +class LazyMapping(Mapping[str, Any]): + """Helper for decoding SQL rows on demand according to a schema.""" + def __init__(self, schema: Dict[str, Type], data: Tuple[Tuple[str, Any], ...]): + self._schema = schema + self._data = dict(data) + + def __getitem__(self, key: str) -> Any: + if key not in self._schema: + raise KeyError(f"Field {key!r} not in map schema.") + if key not in self._data: + raise KeyError(f"Field {key!r} not found in data.") + field_type = self._schema[key] + raw = self._data[key] + if raw is None: + return None + if field_type not in _PY_TO_SQL: + decoded = field_type(cbor2.loads(raw)) + return decoded + if field_type is bool: + return bool(raw) + return raw + + def __iter__(self) -> Iterator[str]: + return iter(self._schema.keys()) + + def __len__(self) -> int: + return len(self._schema) + +class SqliteStore(IStore): + def __init__(self, name: str, path: PathLike): + self._name = name + self._path = path + path_str = self._path.as_str() + if path_str != ":memory:": + parent = os.path.dirname(path_str) + if parent: + os.makedirs(parent, exist_ok=True) + self._conn = sqlite3.connect(path_str, isolation_level=None) + self._conn.execute("PRAGMA journal_mode = WAL;") + self._conn.execute("PRAGMA temp_store = MEMORY;") + self._schemas: Dict[str, Tuple[Dict[str, Type], str]] = {} + self._dirty = False + + def name(self) -> str: + return self._name + + def path(self) -> PathLike: + return self._path + + @property + def type(self) -> StoreType: + return StoreType.DATABASE + + def create_map(self, map_name: str, schema: Dict[str, Type], primary_key: str) -> None: + if map_name in self._schemas: + raise ValueError(f"Map {map_name!r} already exists in store {self._name!r}.") + _validate_struct(schema, primary_key) + col_defs: List[str] = [] + for field, field_type in schema.items(): + sql_type = _PY_TO_SQL.get(field_type, "BLOB") + col_def = f"{field} {sql_type}" + if field == primary_key: + col_def += " PRIMARY KEY" + col_defs.append(col_def) + sql = f"CREATE TABLE {map_name} ({', '.join(col_defs)})" + self._conn.execute(sql) + self._schemas[map_name] = (schema, primary_key) + + def delete_map(self, map_name: str) -> None: + if map_name not in self._schemas: + raise KeyError(f"Map {map_name!r} does not exist in store {self._name!r}.") + self._conn.execute(f"DROP TABLE {map_name}") + del self._schemas[map_name] + + def list_maps(self) -> List[str]: + return list(self._schemas.keys()) + + def _require_schema(self, map_name: str) -> Tuple[Dict[str, Type], str]: + if map_name not in self._schemas: + raise IndexError(f"Map {map_name!r} does not exist in store {self._name!r}.") + return self._schemas[map_name] + + def insert_data(self, map_name: str, struct: Dict[str, Any]) -> None: + schema, pk = self._require_schema(map_name) + if pk not in struct: + raise ValueError(f"Primary key {pk!r} missing from struct.") + + # All schema fields must be present in the struct + values = [] + for key in schema: + if key not in struct: + raise ValueError(f"Field {key!r} missing from struct.") + if schema[key] not in _PY_TO_SQL: + encoded = cbor2.dumps(struct[key]) + values.append(encoded) + elif schema[key] is bool: + values.append(1 if struct[key] else 0) else: - raise FileNotFoundError(f"Store file '{file_path}' does not exist.") - else: - raise NotImplementedError(f"Delete operation not implemented for {self.store_type}.") + values.append(struct[key]) + + cols = ", ".join(schema.keys()) + placeholders = ", ".join(["?"]*len(schema)) + try: + self._conn.execute( + f"INSERT INTO {map_name} ({cols}) VALUES ({placeholders})", + tuple(values), + ) + except sqlite3.IntegrityError as exc: + raise KeyError(f"Key {struct[pk]!r} already exists in map {map_name!r}.") from exc + self._dirty = True + + def get_data(self, map_name: str, key: Any) -> Mapping[str, Any]: + schema, pk = self._require_schema(map_name) + self.flush() + cols = ", ".join(schema.keys()) + cursor = self._conn.execute( + f"SELECT {cols} FROM {map_name} WHERE {pk} = ?", + (key,), + ) + row = cursor.fetchone() + if row is None: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + return LazyMapping(schema, tuple(zip(schema.keys(), row))) + + def update_data(self, map_name: str, struct: Dict[str, Any]) -> None: + schema, pk = self._require_schema(map_name) + if pk not in struct: + raise ValueError(f"Primary key {pk!r} missing from struct.") + key = struct.pop(pk) + # All struct fields must be present in the schema + values = [] + assignments = [] + for field in struct: + if field not in schema: + raise ValueError(f"Field {field!r} not found in schema for map {map_name!r}.") + if schema[field] not in _PY_TO_SQL: + encoded = cbor2.dumps(struct[field]) + values.append(encoded) + elif schema[field] is bool: + values.append(1 if struct[field] else 0) + else: + values.append(struct[field]) + assignments.append(f"{field} = ?") + + assignments_str = ", ".join(assignments) + try: + cursor = self._conn.execute( + f"UPDATE {map_name} SET {assignments_str} WHERE {pk} = ?", + tuple(values + [key]), + ) + except sqlite3.IntegrityError as exc: + raise ValueError(f"Unexpected error occurred while updating map {map_name!r}.") from exc + if cursor.rowcount == 0: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + self._dirty = True + + def delete_data(self, map_name: str, key: Any) -> None: + schema, pk = self._require_schema(map_name) + self.flush() + try: + cursor = self._conn.execute( + f"DELETE FROM {map_name} WHERE {pk} = ?", + (key,), + ) + except sqlite3.IntegrityError as exc: + raise ValueError(f"Unexpected error occurred while deleting from map {map_name!r}.") from exc + if cursor.rowcount == 0: + raise KeyError(f"Key {key!r} not found in map {map_name!r}.") + self._dirty = True + + def query_keys(self, map_name: str) -> Iterator[Any]: + schema, pk = self._require_schema(map_name) + self.flush() + cursor = self._conn.execute( + f"SELECT {pk} FROM {map_name}" + ) + pk_type = schema[pk] + while True: + row = cursor.fetchone() + if row is None: + break + yield pk_type(row[0]) + + # ---- generic extension methods -------------------------------------- + + def connection(self) -> sqlite3.Connection: + return self._conn + + def cursor(self) -> sqlite3.Cursor: + return self._conn.cursor() + + def flush(self) -> None: + if self._dirty: + self._conn.commit() + self._dirty = False + + def mark_dirty(self) -> None: + """Mark pending writes that bypassed the IStore API. + + Consumers using ``connection()`` / ``cursor()`` directly should call + this so subsequent reads ``flush()`` first. + """ + self._dirty = True + + def close(self) -> None: + self.flush() + self._conn.close() \ No newline at end of file diff --git a/gamms/SensorEngine/sensor_engine.py b/gamms/SensorEngine/sensor_engine.py index bafac13..8a8ef5b 100644 --- a/gamms/SensorEngine/sensor_engine.py +++ b/gamms/SensorEngine/sensor_engine.py @@ -1,510 +1,75 @@ -from gamms.typing import( +"""SensorEngine factory. + +The actual sensor classes live in: +- ``sensors_basic`` — NeighborSensor, MapSensor, AgentSensor +- ``sensors_aerial`` — AerialSensor, AerialAgentSensor +- ``sensors_occluded`` — OccludedMapSensor, OccludedAgentSensor, + OccludedAerialSensor, OccludedAerialAgentSensor (one general class per axis; + ARC/RANGE variants are factory presets only). +""" + +import math +from typing import Any, Callable, Dict, cast + +from aenum import extend_enum + +from gamms.typing import ( IContext, ISensor, ISensorEngine, SensorType, - Node, - OSMEdge, - AgentType, - IAerialAgent +) +from gamms.SensorEngine.sensors_basic import AgentSensor, MapSensor, NeighborSensor +from gamms.SensorEngine.sensors_aerial import AerialAgentSensor, AerialSensor +from gamms.SensorEngine.sensors_occluded import ( + OccludedAerialAgentSensor, + OccludedAerialSensor, + OccludedAgentSensor, + OccludedMapSensor, ) -from typing import Any, Dict, Optional, Callable, Tuple, List, Union, cast -from aenum import extend_enum -import math - -class NeighborSensor(ISensor): - def __init__(self, ctx: IContext, sensor_id: str, sensor_type: SensorType): - self._sensor_id = sensor_id - self.ctx = ctx - self._type = sensor_type - self._data = [] - self._owner = None - - @property - def sensor_id(self) -> str: - return self._sensor_id - - @property - def type(self) -> SensorType: - return self._type - - @property - def data(self): - return self._data - - def set_owner(self, owner: Union[str, None]) -> None: - self._owner = owner - - def sense(self, node_id: int) -> None: - nearest_neighbors = {node_id,} - for nid in self.ctx.graph.graph.get_neighbors(node_id): - nearest_neighbors.add(nid) - - self._data = list(nearest_neighbors) - - def update(self, data: Dict[str, Any]) -> None: - pass - -class MapSensor(ISensor): - def __init__(self, ctx: IContext, sensor_id: str, sensor_type: SensorType, sensor_range: float, fov: float, orientation: Tuple[float, float] = (1.0, 0.0)): - """ - Acts as a map sensor (if sensor_range == inf), - a range sensor (if fov == 2*pi), - or a unidirectional sensor (if fov < 2*pi). - Assumes fov and orientation are provided in radians. - """ - self.ctx = ctx - self._sensor_id = sensor_id - self._type = sensor_type - self.range = sensor_range - self.fov = fov - norm = math.sqrt(orientation[0]**2 + orientation[1]**2) - self.orientation = (orientation[0] / norm, orientation[1] / norm) - self._data: Dict[str, Union[Dict[int, Node], List[OSMEdge]]] = {} - # Cache static node IDs and positions. - self._owner = None - - @property - def sensor_id(self) -> str: - return self._sensor_id - - @property - def type(self) -> SensorType: - return self._type - - @property - def data(self) -> Dict[str, Union[Dict[int, Node],List[OSMEdge]]]: - return self._data - - def set_owner(self, owner: Union[str, None]) -> None: - self._owner = owner - - def sense(self, node_id: int) -> None: - """ - Detects nodes within the sensor range and arc. - - The result is now stored in self._data as a dictionary with two keys: - - 'nodes': {node_id: node, ...} for nodes that pass the sensing filter. - - 'edges': List of edges visible from all sensed nodes. - """ - current_node = self.ctx.graph.graph.get_node(node_id) - if self._owner is not None: - # Fetch the owner's orientation from the agent engine. - orientation_used = self.ctx.agent.get_agent(self._owner).orientation - # Complex multiplication to rotate the orientation vector. - orientation_used = ( - self.orientation[0]*orientation_used[0] - self.orientation[1]*orientation_used[1], - self.orientation[0]*orientation_used[1] + self.orientation[1]*orientation_used[0] - ) - else: - orientation_used = self.orientation - - if self.range == float('inf'): - edge_iter = self.ctx.graph.graph.get_edges() - else: - edge_iter = self.ctx.graph.graph.get_edges(d=self.range, x=current_node.x, y=current_node.y) - - - sensed_nodes: Dict[int, Node] = {} - sensed_edges: List[OSMEdge] = [] - - for edge_id in edge_iter: - edge = self.ctx.graph.graph.get_edge(edge_id) - source = self.ctx.graph.graph.get_node(edge.source) - target = self.ctx.graph.graph.get_node(edge.target) - sbool = (source.x - current_node.x)**2 + (source.y - current_node.y)**2 <= self.range**2 - tbool = (target.x - current_node.x)**2 + (target.y - current_node.y)**2 <= self.range**2 - if not (self.fov == 2 * math.pi or orientation_used == (0.0, 0.0)): - angle = math.atan2(source.y - current_node.y, source.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi - angle = angle % (2 * math.pi) - angle = angle - math.pi - sbool &= ( - abs(angle) <= self.fov / 2 - ) or (source.id == node_id) - angle = math.atan2(target.y - current_node.y, target.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi - angle = angle % (2 * math.pi) - angle = angle - math.pi - tbool &= ( - abs(angle) <= self.fov / 2 - ) or (target.id == node_id) - if sbool: - sensed_nodes[source.id] = source - if tbool: - sensed_nodes[target.id] = target - if sbool and tbool: - sensed_edges.append(edge) - - self._data = {'nodes': sensed_nodes, 'edges': sensed_edges} - - def update(self, data: Dict[str, Any]) -> None: - # No dynamic updates required for this sensor. - pass - -class AgentSensor(ISensor): - def __init__( - self, - ctx: IContext, - sensor_id: str, - sensor_type: SensorType, - sensor_range: float, - fov: float = 2 * math.pi, - orientation: Tuple[float, float] = (1.0, 0.0), - owner: Optional[str] = None - ): - """ - Detects other agents within a specified range and field of view. - :param agent_engine: Typically the context's agent engine. - :param sensor_range: Maximum detection distance for agents. - :param fov: Field of view in radians. Use 2*pi for no angular filtering. - :param orientation: Default orientation (in radians) if no owner is set. - :param owner: (Optional) The name of the agent owning this sensor. - This agent will be skipped during sensing. - """ - self._sensor_id = sensor_id - self.ctx = ctx - self._type = sensor_type - self.range = sensor_range - self.fov = fov - self.orientation = orientation - self._owner = owner - self._data: Dict[str, int] = {} - - - @property - def sensor_id(self) -> str: - return self._sensor_id - - @property - def type(self) -> SensorType: - return self._type - - @property - def data(self) -> Dict[str, int]: - return self._data - - def set_owner(self, owner: Union[str, None]) -> None: - self._owner = owner - - def sense(self, node_id: int) -> None: - """ - Detects agents within the sensor range of the sensing node. - Skips the agent whose name matches self._owner. - In addition to a range check, if self.fov != 2*pi, only agents within (fov/2) radians - of the chosen orientation are included. - The chosen orientation is determined as follows: - - If self._owner is set, fetch the owner's orientation from the agent engine. - - Otherwise, use self.orientation. - The result is stored in self._data as a dictionary mapping agent names to agent objects. - """ - # Get current node position as sensing origin. - current_node = self.ctx.graph.graph.get_node(node_id) - - if self._owner is not None: - # Fetch the owner's orientation from the agent engine. - orientation_used = self.ctx.agent.get_agent(self._owner).orientation - # Complex multiplication to rotate the orientation vector. - orientation_used = ( - self.orientation[0]*orientation_used[0] - self.orientation[1]*orientation_used[1], - self.orientation[0]*orientation_used[1] + self.orientation[1]*orientation_used[0] - ) - else: - orientation_used = self.orientation - - sensed_agents = {} - - # Collect positions and ids for all agents except the owner. - for agent in self.ctx.agent.create_iter(): - if agent.name == self._owner: - continue - - agent_node = self.ctx.graph.graph.get_node(agent.current_node_id) - distance = (agent_node.x - current_node.x)**2 + (agent_node.y - current_node.y)**2 - - if distance <= self.range**2: - if self.fov == 2 * math.pi or orientation_used == (0.0, 0.0): - sensed_agents[agent.name] = agent.current_node_id - else: - angle = math.atan2(agent_node.y - current_node.y, agent_node.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi - angle = angle % (2 * math.pi) - angle = angle - math.pi - if abs(angle) <= self.fov / 2 or agent.current_node_id == node_id: - sensed_agents[agent.name] = agent.current_node_id - - self._data = sensed_agents - - def update(self, data: Dict[str, Any]) -> None: - # No dynamic updates required for this sensor. - pass - -def multiply_quaternions(q1: Tuple[float, float, float, float], q2: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]: - w1, x1, y1, z1 = q1 - w2, x2, y2, z2 = q2 - return ( - w1*w2 - x1*x2 - y1*y2 - z1*z2, - w1*x2 + x1*w2 + y1*z2 - z1*y2, - w1*y2 - x1*z2 + y1*w2 + z1*x2, - w1*z2 + x1*y2 - y1*x2 + z1*w2 - ) - -def quaternion_to_direction(quat: Tuple[float, float, float, float]) -> Tuple[float, float, float]: - w, x, y, z = quat - # Convert quaternion to direction vector (assuming forward is along the x-axis) - return ( - 1 - 2*(y**2 + z**2), - 2*(x*y + w*z), - 2*(x*z - w*y) - ) - -class AerialSensor(ISensor): - def __init__( - self, - ctx: IContext, - sensor_id: str, - sensor_range: float, - fov: float = math.pi / 3, - quat: Tuple[float, float, float, float] = (math.sqrt(0.5), 0.0, math.sqrt(0.5), 0.0) - ): # Default 60° FOV - """ - Downward-facing conic sensor for aerial agents. - - Args: - sensor_range: Maximum slant distance from drone to detected point - fov: Field of view angle in radians (half-angle of cone) - """ - self._sensor_id = sensor_id - self.ctx = ctx - self._data: Dict[str, Union[Dict[int, Node], List[OSMEdge]]] = {} - self._owner = None - self.range = sensor_range - self.fov = min(fov, math.pi * 0.9) # Cap at ~162° to avoid backward vision - self.quat = quat - - @property - def sensor_id(self) -> str: - return self._sensor_id - - @property - def type(self) -> SensorType: - return SensorType.AERIAL - - @property - def data(self) -> Dict[str, Union[Dict[int, Node], List[OSMEdge]]]: - return self._data - - def set_owner(self, owner: Union[str, None]) -> None: - if owner is not None: - agent = self.ctx.agent.get_agent(owner) - if agent.type != AgentType.AERIAL: - raise ValueError("Owner of AerialSensor must be an aerial agent") - self._owner = owner - - def sense(self, node_id: int) -> None: - """ - Detect nodes within the conic field of view from the drone's position. - - Args: - node_id: Current node (may not be used if drone is airborne) - """ - # If no owner, return empty - if self._owner is None: - self._data = {'nodes': {}, 'edges': []} - return - agent = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)) - - # Multiply agent orientation by sensor quaternion - orientation = multiply_quaternions(agent.quat, self.quat) - # Convert to Euler angles to extract pitch - fx, fy, fz = quaternion_to_direction(orientation) - - pos = agent.position - - x, y, z = pos - - # Calculate the radius of visibility on the ground - # Based on cone geometry and sensor range constraints - half_angle = self.fov / 2 - - sensed_nodes: Dict[int, Node] = {} - sensed_edges: List[OSMEdge] = [] - - for edge_id in self.ctx.graph.graph.get_edges(d=self.range, x=x, y=y): - edge = self.ctx.graph.graph.get_edge(edge_id) - source = self.ctx.graph.graph.get_node(edge.source) - target = self.ctx.graph.graph.get_node(edge.target) - # Check if either endpoint is within range - normsq = (source.x - x)**2 + (source.y - y)**2 + z**2 - cosine = (source.x - x) * fx + (source.y - y) * fy - z * fz - angle = math.acos(max(min(cosine/math.sqrt(normsq), 1.0), -1.0)) if normsq != 0 else 2*math.pi - sbool = (normsq <= self.range**2) and (angle <= half_angle) - normsq = (target.x - x)**2 + (target.y - y)**2 + z**2 - cosine = (target.x - x) * fx + (target.y - y) * fy - z * fz - angle = math.acos(max(min(cosine/math.sqrt(normsq), 1.0), -1.0)) if normsq != 0 else 2*math.pi - tbool = (normsq <= self.range**2) and (angle <= half_angle) - # Check if angle between node vector and downward vertical is within FOV - if sbool: - sensed_nodes[source.id] = source - if tbool: - sensed_nodes[target.id] = target - if sbool and tbool: - sensed_edges.append(edge) - - self._data = {'nodes': sensed_nodes, 'edges': sensed_edges} - - def update(self, data: Dict[str, Any]) -> None: - pass - - -class AerialAgentSensor(ISensor): - def __init__( - self, - ctx: IContext, - sensor_id: str, - sensor_range: float, - fov: float = 2 * math.pi, - quat: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) - ): - """ - Detects other aerial agents within a specified 3D range and field of view. - Similar to AgentSensor but works in 3D space for aerial agents. - - Args: - sensor_range: Maximum detection distance for agents - fov: Field of view in radians. Use 2*pi for no angular filtering - quat: Default quaternion (w, x, y, z) if no owner is set - """ - self._sensor_id = sensor_id - self.ctx = ctx - self.range = sensor_range - self.fov = fov - self.quat = quat - self._owner = None - self._data: Dict[str, Tuple[AgentType, Tuple[float, float, float]]] = {} - - @property - def sensor_id(self) -> str: - return self._sensor_id - - @property - def type(self) -> SensorType: - return SensorType.AERIAL_AGENT - - @property - def data(self) -> Dict[str, Tuple[float, float, float]]: - return self._data - - def set_owner(self, owner: Union[str, None]) -> None: - agent = self.ctx.agent.get_agent(owner) if owner else None - if agent is not None: - if agent.type != AgentType.AERIAL: - raise ValueError("Owner of AerialAgentSensor must be an aerial agent") - self._owner = owner - - def sense(self, node_id: int) -> None: - """ - Detects agents within the sensor range in 3D space. - Returns agent positions instead of node IDs for aerial agents. - """ - # Get sensing position - if self._owner is None: - self._data = {} - return - agent = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)) - quat = multiply_quaternions(agent.quat, self.quat) - fx, fy, fz = quaternion_to_direction(quat) - - x, y, z = agent.position - - sensed_agents = {} - - # Check all agents except the owner - for agent in self.ctx.agent.create_iter(): - if agent.name == self._owner: - continue - - # Get agent position - if agent.type == AgentType.AERIAL: - agent_pos = cast(IAerialAgent, agent).position - elif agent.type == AgentType.BASIC: - agent_node = self.ctx.graph.graph.get_node(agent.current_node_id) - agent_pos = (agent_node.x, agent_node.y, 0.0) - else: - raise RuntimeError(f"Unknown agent type {agent.type} for agent {agent.name}") - - # Calculate 3D distance - dx = agent_pos[0] - x - dy = agent_pos[1] - y - dz = agent_pos[2] - z - distance_3d = dx**2 + dy**2 + dz**2 - - cosine = dx * fx + dy * fy + dz * fz - angle = math.acos(max(min(cosine/math.sqrt(distance_3d), 1.0), -1.0)) if distance_3d != 0 else 2*math.pi - # Check range and FOV - agent_bool = (distance_3d <= self.range**2) and (angle <= self.fov / 2) - - if agent_bool: - sensed_agents[agent.name] = (agent.type, agent_pos) - - self._data = sensed_agents - - def update(self, data: Dict[str, Any]) -> None: - pass class SensorEngine(ISensorEngine): def __init__(self, ctx: IContext): - self.ctx = ctx + self.ctx = ctx self.sensors: Dict[str, ISensor] = {} def create_sensor(self, sensor_id: str, sensor_type: SensorType, **kwargs: Dict[str, Any]) -> ISensor: if sensor_type == SensorType.NEIGHBOR: - sensor = NeighborSensor( - self.ctx, sensor_id, sensor_type, - ) + sensor: ISensor = NeighborSensor(self.ctx, sensor_id, sensor_type) elif sensor_type == SensorType.MAP: sensor = MapSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=float('inf'), fov=2 * math.pi, ) elif sensor_type == SensorType.RANGE: sensor = MapSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=cast(float, kwargs.get('sensor_range', 30.0)), - fov=(2 * math.pi), + fov=2 * math.pi, ) elif sensor_type == SensorType.ARC: sensor = MapSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=cast(float, kwargs.get('sensor_range', 30.0)), fov=cast(float, kwargs.get('fov', 2 * math.pi)), ) elif sensor_type == SensorType.AGENT: sensor = AgentSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=float('inf'), fov=cast(float, kwargs.get('fov', 2 * math.pi)), ) elif sensor_type == SensorType.AGENT_ARC: sensor = AgentSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=cast(float, kwargs.get('sensor_range', 30.0)), - fov=cast(float, kwargs.get('fov', 2 * math.pi)), + fov=cast(float, kwargs.get('fov', 2 * math.pi)), ) elif sensor_type == SensorType.AGENT_RANGE: sensor = AgentSensor( - self.ctx, - sensor_id, - sensor_type, + self.ctx, sensor_id, sensor_type, sensor_range=cast(float, kwargs.get('sensor_range', 30.0)), fov=2 * math.pi, ) @@ -512,21 +77,49 @@ def create_sensor(self, sensor_id: str, sensor_type: SensorType, **kwargs: Dict[ sensor = AerialSensor( self.ctx, sensor_id, sensor_range=cast(float, kwargs.get('sensor_range', 100.0)), - fov=cast(float, kwargs.get('fov', math.pi/3)), # Default 60° FOV - quat=kwargs.get('quat', (0.0, 0.0, 1.0, 0.0)) # Default downward-facing + fov=cast(float, kwargs.get('fov', math.pi / 3)), + quat=cast(tuple, kwargs.get('quat', (0.0, 0.0, 1.0, 0.0))), ) elif sensor_type == SensorType.AERIAL_AGENT: sensor = AerialAgentSensor( self.ctx, sensor_id, sensor_range=cast(float, kwargs.get('sensor_range', 100.0)), fov=cast(float, kwargs.get('fov', 2 * math.pi)), - quat=kwargs.get('quat', (1.0, 0.0, 0.0, 0.0)) + quat=cast(tuple, kwargs.get('quat', (1.0, 0.0, 0.0, 0.0))), + ) + elif sensor_type == SensorType.OCCLUDED_MAP: + sensor = OccludedMapSensor( + self.ctx, sensor_id, sensor_type, + sensor_range=cast(float, kwargs.get('sensor_range', float('inf'))), + fov=cast(float, kwargs.get('fov', 2 * math.pi)), + observer_height=cast(float, kwargs.get('observer_height', 1.6)), + ) + elif sensor_type == SensorType.OCCLUDED_AGENT: + sensor = OccludedAgentSensor( + self.ctx, sensor_id, sensor_type, + sensor_range=cast(float, kwargs.get('sensor_range', float('inf'))), + fov=cast(float, kwargs.get('fov', 2 * math.pi)), + observer_height=cast(float, kwargs.get('observer_height', 1.6)), + ) + elif sensor_type == SensorType.OCCLUDED_AERIAL: + sensor = OccludedAerialSensor( + self.ctx, sensor_id, + sensor_range=cast(float, kwargs.get('sensor_range', 100.0)), + fov=cast(float, kwargs.get('fov', math.pi / 3)), + quat=cast(tuple, kwargs.get('quat', (math.sqrt(0.5), 0.0, math.sqrt(0.5), 0.0))), + ) + elif sensor_type == SensorType.OCCLUDED_AERIAL_AGENT: + sensor = OccludedAerialAgentSensor( + self.ctx, sensor_id, + sensor_range=cast(float, kwargs.get('sensor_range', 100.0)), + fov=cast(float, kwargs.get('fov', 2 * math.pi)), + quat=cast(tuple, kwargs.get('quat', (1.0, 0.0, 0.0, 0.0))), ) else: raise ValueError("Invalid sensor type") self.add_sensor(sensor) return sensor - + def add_sensor(self, sensor: ISensor) -> None: sensor_id = sensor.sensor_id if sensor_id in self.sensors: @@ -545,6 +138,7 @@ def custom(self, name: str) -> Callable[[ISensor], ISensor]: else: extend_enum(SensorType, name, len(SensorType)) val = getattr(SensorType, name) + def decorator(cls_type: ISensor) -> ISensor: cls_type.type = property(lambda obj: val) return cls_type diff --git a/gamms/SensorEngine/sensors_aerial.py b/gamms/SensorEngine/sensors_aerial.py new file mode 100644 index 0000000..775247d --- /dev/null +++ b/gamms/SensorEngine/sensors_aerial.py @@ -0,0 +1,191 @@ +"""Aerial sensors: AerialSensor, AerialAgentSensor, plus quaternion helpers.""" + +import math +from typing import Any, Dict, List, Optional, Tuple, Union, cast + +from gamms.typing import ( + AgentType, + IAerialAgent, + IContext, + ISensor, + Node, + OSMEdge, + SensorType, +) + + +def multiply_quaternions( + q1: Tuple[float, float, float, float], + q2: Tuple[float, float, float, float], +) -> Tuple[float, float, float, float]: + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return ( + w1*w2 - x1*x2 - y1*y2 - z1*z2, + w1*x2 + x1*w2 + y1*z2 - z1*y2, + w1*y2 - x1*z2 + y1*w2 + z1*x2, + w1*z2 + x1*y2 - y1*x2 + z1*w2, + ) + + +def quaternion_to_direction( + quat: Tuple[float, float, float, float], +) -> Tuple[float, float, float]: + w, x, y, z = quat + return ( + 1 - 2*(y**2 + z**2), + 2*(x*y + w*z), + 2*(x*z - w*y), + ) + + +class AerialSensor(ISensor): + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_range: float, + fov: float = math.pi / 3, + quat: Tuple[float, float, float, float] = (math.sqrt(0.5), 0.0, math.sqrt(0.5), 0.0), + ): + """ + Downward-facing conic sensor for aerial agents. + """ + self._sensor_id = sensor_id + self.ctx = ctx + self._data: Dict[str, Union[Dict[int, Node], List[OSMEdge]]] = {} + self._owner: Optional[str] = None + self.range = sensor_range + self.fov = min(fov, math.pi * 0.9) + self.quat = quat + + @property + def sensor_id(self) -> str: + return self._sensor_id + + @property + def type(self) -> SensorType: + return SensorType.AERIAL + + @property + def data(self) -> Dict[str, Union[Dict[int, Node], List[OSMEdge]]]: + return self._data + + def set_owner(self, owner: Union[str, None]) -> None: + if owner is not None: + agent = self.ctx.agent.get_agent(owner) + if agent.type != AgentType.AERIAL: + raise ValueError("Owner of AerialSensor must be an aerial agent") + self._owner = owner + + def sense(self, node_id: int) -> None: + if self._owner is None: + self._data = {'nodes': {}, 'edges': []} + return + agent = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)) + orientation = multiply_quaternions(agent.quat, self.quat) + fx, fy, fz = quaternion_to_direction(orientation) + x, y, z = agent.position + half_angle = self.fov / 2 + + sensed_nodes: Dict[int, Node] = {} + sensed_edges: List[OSMEdge] = [] + + for edge_id in self.ctx.graph.graph.get_edges(d=self.range, x=x, y=y): + edge = self.ctx.graph.graph.get_edge(edge_id) + source = self.ctx.graph.graph.get_node(edge.source) + target = self.ctx.graph.graph.get_node(edge.target) + + normsq = (source.x - x)**2 + (source.y - y)**2 + z**2 + cosine = (source.x - x) * fx + (source.y - y) * fy - z * fz + angle = math.acos(max(min(cosine / math.sqrt(normsq), 1.0), -1.0)) if normsq != 0 else 2 * math.pi + sbool = (normsq <= self.range**2) and (angle <= half_angle) + + normsq = (target.x - x)**2 + (target.y - y)**2 + z**2 + cosine = (target.x - x) * fx + (target.y - y) * fy - z * fz + angle = math.acos(max(min(cosine / math.sqrt(normsq), 1.0), -1.0)) if normsq != 0 else 2 * math.pi + tbool = (normsq <= self.range**2) and (angle <= half_angle) + + if sbool: + sensed_nodes[source.id] = source + if tbool: + sensed_nodes[target.id] = target + if sbool and tbool: + sensed_edges.append(edge) + + self._data = {'nodes': sensed_nodes, 'edges': sensed_edges} + + def update(self, data: Dict[str, Any]) -> None: + pass + + +class AerialAgentSensor(ISensor): + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_range: float, + fov: float = 2 * math.pi, + quat: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0), + ): + self._sensor_id = sensor_id + self.ctx = ctx + self.range = sensor_range + self.fov = fov + self.quat = quat + self._owner: Optional[str] = None + self._data: Dict[str, Tuple[AgentType, Tuple[float, float, float]]] = {} + + @property + def sensor_id(self) -> str: + return self._sensor_id + + @property + def type(self) -> SensorType: + return SensorType.AERIAL_AGENT + + @property + def data(self) -> Dict[str, Tuple[AgentType, Tuple[float, float, float]]]: + return self._data + + def set_owner(self, owner: Union[str, None]) -> None: + agent = self.ctx.agent.get_agent(owner) if owner else None + if agent is not None: + if agent.type != AgentType.AERIAL: + raise ValueError("Owner of AerialAgentSensor must be an aerial agent") + self._owner = owner + + def sense(self, node_id: int) -> None: + if self._owner is None: + self._data = {} + return + agent = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)) + quat = multiply_quaternions(agent.quat, self.quat) + fx, fy, fz = quaternion_to_direction(quat) + x, y, z = agent.position + + sensed_agents: Dict[str, Tuple[AgentType, Tuple[float, float, float]]] = {} + for other in self.ctx.agent.create_iter(): + if other.name == self._owner: + continue + if other.type == AgentType.AERIAL: + agent_pos = cast(IAerialAgent, other).position + elif other.type == AgentType.BASIC: + agent_node = self.ctx.graph.graph.get_node(other.current_node_id) + agent_pos = (agent_node.x, agent_node.y, 0.0) + else: + raise RuntimeError(f"Unknown agent type {other.type} for agent {other.name}") + + dx = agent_pos[0] - x + dy = agent_pos[1] - y + dz = agent_pos[2] - z + distance_3d = dx**2 + dy**2 + dz**2 + cosine = dx * fx + dy * fy + dz * fz + angle = math.acos(max(min(cosine / math.sqrt(distance_3d), 1.0), -1.0)) if distance_3d != 0 else 2 * math.pi + if (distance_3d <= self.range**2) and (angle <= self.fov / 2): + sensed_agents[other.name] = (other.type, agent_pos) + + self._data = sensed_agents + + def update(self, data: Dict[str, Any]) -> None: + pass diff --git a/gamms/SensorEngine/sensors_basic.py b/gamms/SensorEngine/sensors_basic.py new file mode 100644 index 0000000..1dd3389 --- /dev/null +++ b/gamms/SensorEngine/sensors_basic.py @@ -0,0 +1,201 @@ +"""Basic ground-level sensors: NeighborSensor, MapSensor, AgentSensor.""" + +import math +from typing import Any, Dict, List, Optional, Tuple, Union + +from gamms.typing import ( + IContext, + ISensor, + Node, + OSMEdge, + SensorType, +) + + +class NeighborSensor(ISensor): + def __init__(self, ctx: IContext, sensor_id: str, sensor_type: SensorType): + self._sensor_id = sensor_id + self.ctx = ctx + self._type = sensor_type + self._data: List[int] = [] + self._owner: Optional[str] = None + + @property + def sensor_id(self) -> str: + return self._sensor_id + + @property + def type(self) -> SensorType: + return self._type + + @property + def data(self): + return self._data + + def set_owner(self, owner: Union[str, None]) -> None: + self._owner = owner + + def sense(self, node_id: int) -> None: + nearest_neighbors = {node_id} + for nid in self.ctx.graph.graph.get_neighbors(node_id): + nearest_neighbors.add(nid) + self._data = list(nearest_neighbors) + + def update(self, data: Dict[str, Any]) -> None: + pass + + +class MapSensor(ISensor): + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_type: SensorType, + sensor_range: float, + fov: float, + orientation: Tuple[float, float] = (1.0, 0.0), + ): + """ + Acts as a map sensor (range == inf), a range sensor (fov == 2π), + or a unidirectional sensor (fov < 2π). FOV/orientation in radians. + """ + self.ctx = ctx + self._sensor_id = sensor_id + self._type = sensor_type + self.range = sensor_range + self.fov = fov + norm = math.sqrt(orientation[0]**2 + orientation[1]**2) + self.orientation = (orientation[0] / norm, orientation[1] / norm) + self._data: Dict[str, Union[Dict[int, Node], List[OSMEdge]]] = {} + self._owner: Optional[str] = None + + @property + def sensor_id(self) -> str: + return self._sensor_id + + @property + def type(self) -> SensorType: + return self._type + + @property + def data(self) -> Dict[str, Union[Dict[int, Node], List[OSMEdge]]]: + return self._data + + def set_owner(self, owner: Union[str, None]) -> None: + self._owner = owner + + def sense(self, node_id: int) -> None: + current_node = self.ctx.graph.graph.get_node(node_id) + if self._owner is not None: + owner_orientation = self.ctx.agent.get_agent(self._owner).orientation + orientation_used = ( + self.orientation[0] * owner_orientation[0] - self.orientation[1] * owner_orientation[1], + self.orientation[0] * owner_orientation[1] + self.orientation[1] * owner_orientation[0], + ) + else: + orientation_used = self.orientation + + if self.range == float('inf'): + edge_iter = self.ctx.graph.graph.get_edges() + else: + edge_iter = self.ctx.graph.graph.get_edges(d=self.range, x=current_node.x, y=current_node.y) + + sensed_nodes: Dict[int, Node] = {} + sensed_edges: List[OSMEdge] = [] + + range_sq = self.range ** 2 if self.range != float('inf') else float('inf') + + for edge_id in edge_iter: + edge = self.ctx.graph.graph.get_edge(edge_id) + source = self.ctx.graph.graph.get_node(edge.source) + target = self.ctx.graph.graph.get_node(edge.target) + sbool = (source.x - current_node.x)**2 + (source.y - current_node.y)**2 <= range_sq + tbool = (target.x - current_node.x)**2 + (target.y - current_node.y)**2 <= range_sq + if not (self.fov == 2 * math.pi or orientation_used == (0.0, 0.0)): + angle = math.atan2(source.y - current_node.y, source.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi + angle = (angle % (2 * math.pi)) - math.pi + sbool &= (abs(angle) <= self.fov / 2) or (source.id == node_id) + angle = math.atan2(target.y - current_node.y, target.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi + angle = (angle % (2 * math.pi)) - math.pi + tbool &= (abs(angle) <= self.fov / 2) or (target.id == node_id) + if sbool: + sensed_nodes[source.id] = source + if tbool: + sensed_nodes[target.id] = target + if sbool and tbool: + sensed_edges.append(edge) + + self._data = {'nodes': sensed_nodes, 'edges': sensed_edges} + + def update(self, data: Dict[str, Any]) -> None: + pass + + +class AgentSensor(ISensor): + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_type: SensorType, + sensor_range: float, + fov: float = 2 * math.pi, + orientation: Tuple[float, float] = (1.0, 0.0), + owner: Optional[str] = None, + ): + self._sensor_id = sensor_id + self.ctx = ctx + self._type = sensor_type + self.range = sensor_range + self.fov = fov + self.orientation = orientation + self._owner = owner + self._data: Dict[str, int] = {} + + @property + def sensor_id(self) -> str: + return self._sensor_id + + @property + def type(self) -> SensorType: + return self._type + + @property + def data(self) -> Dict[str, int]: + return self._data + + def set_owner(self, owner: Union[str, None]) -> None: + self._owner = owner + + def sense(self, node_id: int) -> None: + current_node = self.ctx.graph.graph.get_node(node_id) + if self._owner is not None: + owner_orientation = self.ctx.agent.get_agent(self._owner).orientation + orientation_used = ( + self.orientation[0] * owner_orientation[0] - self.orientation[1] * owner_orientation[1], + self.orientation[0] * owner_orientation[1] + self.orientation[1] * owner_orientation[0], + ) + else: + orientation_used = self.orientation + + sensed_agents: Dict[str, int] = {} + range_sq = self.range ** 2 if self.range != float('inf') else float('inf') + + for agent in self.ctx.agent.create_iter(): + if agent.name == self._owner: + continue + agent_node = self.ctx.graph.graph.get_node(agent.current_node_id) + distance_sq = (agent_node.x - current_node.x)**2 + (agent_node.y - current_node.y)**2 + + if distance_sq <= range_sq: + if self.fov == 2 * math.pi or orientation_used == (0.0, 0.0): + sensed_agents[agent.name] = agent.current_node_id + else: + angle = math.atan2(agent_node.y - current_node.y, agent_node.x - current_node.x) - math.atan2(orientation_used[1], orientation_used[0]) + math.pi + angle = (angle % (2 * math.pi)) - math.pi + if abs(angle) <= self.fov / 2 or agent.current_node_id == node_id: + sensed_agents[agent.name] = agent.current_node_id + + self._data = sensed_agents + + def update(self, data: Dict[str, Any]) -> None: + pass diff --git a/gamms/SensorEngine/sensors_occluded.py b/gamms/SensorEngine/sensors_occluded.py new file mode 100644 index 0000000..80d8823 --- /dev/null +++ b/gamms/SensorEngine/sensors_occluded.py @@ -0,0 +1,318 @@ +"""Occlusion-aware sensors and the geometry primitives they depend on.""" + +import math +from typing import Any, Dict, Iterator, List, Optional, Tuple, cast + +import numpy as np + +from gamms.typing import ( + AgentType, + IAerialAgent, + IContext, + Node, + SensorType, + ObsFace +) +from gamms.SensorEngine.sensors_basic import AgentSensor, MapSensor +from gamms.SensorEngine.sensors_aerial import AerialAgentSensor, AerialSensor + + +Vec3 = Tuple[float, float, float] + +_FACE_BATCH = 64 # faces processed per numpy kernel call + + +# --------------------------------------------------------------------------- +# Scalar Möller-Trumbore — used by agent sensors (early-exit per agent) +# --------------------------------------------------------------------------- + +def _segment_triangle( + a: Vec3, b: Vec3, + v0: Vec3, v1: Vec3, v2: Vec3, +) -> bool: + """True if segment a→b intersects triangle v0-v1-v2.""" + EPS = 1e-9 + dx, dy, dz = b[0] - a[0], b[1] - a[1], b[2] - a[2] + e1x = v1[0] - v0[0]; e1y = v1[1] - v0[1]; e1z = v1[2] - v0[2] + e2x = v2[0] - v0[0]; e2y = v2[1] - v0[1]; e2z = v2[2] - v0[2] + hx = dy * e2z - dz * e2y + hy = dz * e2x - dx * e2z + hz = dx * e2y - dy * e2x + a_val = e1x * hx + e1y * hy + e1z * hz + if abs(a_val) < EPS: + return False + f = 1.0 / a_val + sx = a[0] - v0[0]; sy = a[1] - v0[1]; sz = a[2] - v0[2] + u = f * (sx * hx + sy * hy + sz * hz) + if u < 0.0 or u > 1.0: + return False + qx = sy * e1z - sz * e1y + qy = sz * e1x - sx * e1z + qz = sx * e1y - sy * e1x + v = f * (dx * qx + dy * qy + dz * qz) + if v < 0.0 or u + v > 1.0: + return False + t = f * (e2x * qx + e2y * qy + e2z * qz) + return 0.0 <= t <= 1.0 + + +def _quad_blocks(a: Vec3, b: Vec3, face: Any) -> bool: + """True if segment a→b is blocked by the ObsFace quad.""" + tl, tr, br, bl = face.tl, face.tr, face.br, face.bl + return _segment_triangle(a, b, tl, tr, br) or _segment_triangle(a, b, tl, br, bl) + + +# --------------------------------------------------------------------------- +# Vectorised kernels — N nodes × 1 face (kept for tests) +# --------------------------------------------------------------------------- + +def _triangle_blocks_batch( + O: np.ndarray, # (3,) + T: np.ndarray, # (N, 3) + v0: np.ndarray, # (3,) + v1: np.ndarray, # (3,) + v2: np.ndarray, # (3,) +) -> np.ndarray: # (N,) bool + """Which of the N segments O→T[i] hit the triangle.""" + EPS = 1e-9 + D = T - O + e1 = v1 - v0 + e2 = v2 - v0 + h = np.cross(D, e2) + a = h @ e1 + valid = np.abs(a) > EPS + result = np.zeros(len(T), dtype=bool) + if not valid.any(): + return result + inv_a = np.where(valid, 1.0 / np.where(valid, a, 1.0), 0.0) + s = O - v0 + u = inv_a * (h @ s) + q = np.cross(s, e1) + v = inv_a * (D @ q) + t = inv_a * float(np.dot(e2, q)) + return valid & (u >= 0.0) & (u <= 1.0) & (v >= 0.0) & (u + v <= 1.0) & (t >= 0.0) & (t <= 1.0) + + +def _quad_blocks_batch( + O: np.ndarray, # (3,) + T: np.ndarray, # (N, 3) + face: Any, +) -> np.ndarray: # (N,) bool + """Which of the N segments O→T[i] are blocked by the ObsFace quad.""" + if len(T) == 0: + return np.zeros(0, dtype=bool) + tl = np.array(face.tl, dtype=float) + tr = np.array(face.tr, dtype=float) + br = np.array(face.br, dtype=float) + bl = np.array(face.bl, dtype=float) + blocked = _triangle_blocks_batch(O, T, tl, tr, br) + remaining = ~blocked + if remaining.any(): + blocked[remaining] = _triangle_blocks_batch(O, T[remaining], tl, br, bl) + return blocked + + +# --------------------------------------------------------------------------- +# Vectorised kernel — F faces × N nodes (map/aerial sensor batch path) +# --------------------------------------------------------------------------- + +def _tri_block_FN( + obs: np.ndarray, # (3,) + targets: np.ndarray, # (N, 3) + v0: np.ndarray, # (F, 3) + v1: np.ndarray, # (F, 3) + v2: np.ndarray, # (F, 3) +) -> np.ndarray: # (F, N) bool + """Möller-Trumbore for F triangles against N segments simultaneously.""" + EPS = 1e-9 + D = targets - obs + e1 = v1 - v0 + e2 = v2 - v0 + h = np.cross(D[np.newaxis], e2[:, np.newaxis]) # (F, N, 3) + a = np.einsum('fni,fi->fn', h, e1) # (F, N) + valid = np.abs(a) > EPS + inv_a = np.where(valid, 1.0 / np.where(valid, a, 1.0), 0.0) + s = obs - v0 # (F, 3) + u = inv_a * np.einsum('fni,fi->fn', h, s) # (F, N) + q = np.cross(s, e1) # (F, 3) + v = inv_a * (D @ q.T).T # (F, N) + t = inv_a * np.einsum('fi,fi->f', e2, q)[:, np.newaxis] + return valid & (u >= 0) & (u <= 1) & (v >= 0) & (u + v <= 1) & (t >= 0) & (t <= 1) + + +def _chunk_blocks( + obs: np.ndarray, # (3,) + targets: np.ndarray, # (N, 3) + chunk: np.ndarray, # (4, F, 3) +) -> np.ndarray: # (N,) bool + """Vectorise a batch of <= _FACE_BATCH faces against N targets.""" + return ( + _tri_block_FN(obs, targets, chunk[0], chunk[1], chunk[2]) | + _tri_block_FN(obs, targets, chunk[0], chunk[2], chunk[3]) + ).any(axis=0) # type: ignore + + +# --------------------------------------------------------------------------- +# Shared helpers +# --------------------------------------------------------------------------- + +def _iter_faces(ctx: IContext, x: float, y: float, radius: float) -> Iterator[ObsFace]: + """Stream ObsFace objects from the graph engine, spatially filtered.""" + for face_id in ctx.graph.get_obstacle_faces(d=radius, x=x, y=y): + face = ctx.graph.get_obstacle_face(face_id) + yield face + +def _apply_occlusion( + obs: np.ndarray, + targets: np.ndarray, + face_iter: Iterator[ObsFace], +) -> np.ndarray: + """Stream faces in chunks, returning a (N,) visible bool array.""" + visible = np.ones(len(targets), dtype=bool) + chunk = np.empty((4, _FACE_BATCH, 3), dtype=float) + idx = 0 + for face in face_iter: + chunk[0, idx, :] = np.asarray(face.tl, dtype=float) + chunk[1, idx, :] = np.asarray(face.tr, dtype=float) + chunk[2, idx, :] = np.asarray(face.br, dtype=float) + chunk[3, idx, :] = np.asarray(face.bl, dtype=float) + idx += 1 + if idx == _FACE_BATCH: + idx = 0 + visible &= ~_chunk_blocks(obs, targets, chunk) + if not visible.any(): + return visible + if idx > 0: + visible &= ~_chunk_blocks(obs, targets, chunk[:, :idx]) + return visible + + +def _filter_data(data: dict, node_ids: list, visible: np.ndarray) -> dict: + """Rebuild sensor _data keeping only visible nodes and edges between them.""" + visible_ids = {node_ids[i] for i, v in enumerate(visible) if v} + return { + 'nodes': {nid: data['nodes'][nid] for nid in visible_ids}, + 'edges': [e for e in data.get('edges', []) + if e.source in visible_ids and e.target in visible_ids], + } + + +# --------------------------------------------------------------------------- +# Sensor classes +# --------------------------------------------------------------------------- + +class OccludedMapSensor(MapSensor): + """MapSensor that drops nodes/edges occluded by obstacle faces.""" + + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_type: SensorType, + sensor_range: float, + fov: float, + orientation: Tuple[float, float] = (1.0, 0.0), + observer_height: float = 1.6, + ) -> None: + super().__init__(ctx, sensor_id, sensor_type, sensor_range, fov, orientation) + self.observer_height = observer_height + + def sense(self, node_id: int) -> None: + super().sense(node_id) + nodes: Dict[int, Node] = cast(Dict[int, Node], self._data.get('nodes')) + if not nodes: + return + + current_node = self.ctx.graph.graph.get_node(node_id) + origin: Vec3 = (current_node.x, current_node.y, self.observer_height) + node_ids = list(nodes) + obs = np.array(origin, dtype=float) + targets = np.array( + [(nodes[nid].x, nodes[nid].y, self.observer_height) for nid in node_ids], + dtype=float, + ) + visible = _apply_occlusion( + obs, targets, + _iter_faces(self.ctx, origin[0], origin[1], self.range), + ) + self._data = _filter_data(self._data, node_ids, visible) + + +class OccludedAgentSensor(AgentSensor): + """AgentSensor that drops agents hidden behind obstacle faces.""" + + def __init__( + self, + ctx: IContext, + sensor_id: str, + sensor_type: SensorType, + sensor_range: float, + fov: float = 2 * math.pi, + orientation: Tuple[float, float] = (1.0, 0.0), + owner: Optional[str] = None, + observer_height: float = 1.6, + ) -> None: + super().__init__(ctx, sensor_id, sensor_type, sensor_range, fov, orientation, owner) + self.observer_height = observer_height + + def sense(self, node_id: int) -> None: + super().sense(node_id) + if not self._data: + return + + current_node = self.ctx.graph.graph.get_node(node_id) + origin: Vec3 = (current_node.x, current_node.y, self.observer_height) + faces = list(_iter_faces(self.ctx, origin[0], origin[1], self.range)) + + kept: Dict[str, int] = {} + for agent_name, agent_node_id in self._data.items(): + agent_node = self.ctx.graph.graph.get_node(agent_node_id) + target = (agent_node.x, agent_node.y, self.observer_height) + if not any(_quad_blocks(origin, target, f) for f in faces): + kept[agent_name] = agent_node_id + self._data = kept + + +class OccludedAerialSensor(AerialSensor): + """AerialSensor that drops ground nodes/edges occluded by obstacle faces.""" + + def sense(self, node_id: int) -> None: + super().sense(node_id) + if self._owner is None: + return + nodes: Dict[int, Node] = cast(Dict[int, Node], self._data.get('nodes')) + if not nodes: + return + + origin = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)).position + node_ids = list(nodes) + obs = np.array(origin, dtype=float) + targets = np.array( + [(nodes[nid].x, nodes[nid].y, 0.0) for nid in node_ids], + dtype=float, + ) + visible = _apply_occlusion( + obs, targets, + _iter_faces(self.ctx, origin[0], origin[1], self.range), + ) + self._data = _filter_data(self._data, node_ids, visible) + + +class OccludedAerialAgentSensor(AerialAgentSensor): + """AerialAgentSensor that drops occluded agents — early exit per agent.""" + + def sense(self, node_id: int) -> None: + super().sense(node_id) + if self._owner is None: + return + if not self._data: + return + + origin = cast(IAerialAgent, self.ctx.agent.get_agent(self._owner)).position + faces = list(_iter_faces(self.ctx, origin[0], origin[1], self.range)) + + kept: Dict[str, Tuple[AgentType, Tuple[float, float, float]]] = {} + for name, (atype, pos) in self._data.items(): + if not any(_quad_blocks(origin, pos, f) for f in faces): + kept[name] = (atype, pos) + self._data = kept diff --git a/gamms/VisualizationEngine/__init__.py b/gamms/VisualizationEngine/__init__.py index e44f1d5..4f5d743 100644 --- a/gamms/VisualizationEngine/__init__.py +++ b/gamms/VisualizationEngine/__init__.py @@ -31,20 +31,33 @@ class Shape(Enum): Circle = auto() Rectangle = auto() + +SHORT_EDGE_PIXEL_THRESHOLD = 3.0 +SKIP_EDGE_PIXEL_THRESHOLD = 1.0 +SKIP_NODE_PIXEL_THRESHOLD = 1.0 +CACHE_ZOOM_MAX = 1.2 + + import sys -import importlib.util - -def lazy(fullname: str): - try: - return sys.modules[fullname] - except KeyError: - spec = importlib.util.find_spec(fullname) - module = importlib.util.module_from_spec(spec) - loader = importlib.util.LazyLoader(spec.loader) - # Make module with proper locking and get it inserted into sys.modules. - loader.exec_module(module) - return module - -from .artist import Artist +import importlib + +def lazy(name: str): + import importlib + module = None + + class _Lazy: + def _load(self): + nonlocal module + if module is None: + module = importlib.import_module(name) + self.__dict__.update(module.__dict__) + + def __getattr__(self, attr): + self._load() + return getattr(module, attr) + + return _Lazy() + +from .artist import Artist, RenderMode from .no_engine import NoEngine -from .pygame_engine import PygameVisualizationEngine \ No newline at end of file +from .pygame_engine import PygameVisualizationEngine diff --git a/gamms/VisualizationEngine/artist.py b/gamms/VisualizationEngine/artist.py index 36d7f1e..020af04 100644 --- a/gamms/VisualizationEngine/artist.py +++ b/gamms/VisualizationEngine/artist.py @@ -1,8 +1,14 @@ +from enum import Enum, auto + from gamms.typing import IArtist, ArtistType, IContext from gamms.VisualizationEngine.default_drawers import render_circle, render_rectangle from gamms.VisualizationEngine import Shape from typing import Callable, Union, Dict, Any +class RenderMode(Enum): + CACHED = auto() + NON_CACHED = auto() + class Artist(IArtist): def __init__(self, ctx: IContext, drawer: Union[Callable[[IContext, Dict[str, Any]], None], Shape], layer: int = 30): self.data = {} @@ -11,8 +17,8 @@ def __init__(self, ctx: IContext, drawer: Union[Callable[[IContext, Dict[str, An self._layer = layer self._layer_dirty = False self._visible = True - self._will_draw = True - self._artist_type = ArtistType.GENERAL + self._artist_type = ArtistType.DYNAMIC + self._render_mode: RenderMode = RenderMode.NON_CACHED if isinstance(drawer, Shape): if drawer == Shape.Circle: self._drawer = render_circle @@ -53,18 +59,30 @@ def set_drawer(self, drawer: Callable[[IContext, Dict[str, Any]], None]): def get_drawer(self) -> Callable[[IContext, Dict[str, Any]], None]: return self._drawer - def get_will_draw(self) -> bool: - return self._will_draw - - def set_will_draw(self, will_draw: bool): - self._will_draw = will_draw - def get_artist_type(self) -> ArtistType: return self._artist_type def set_artist_type(self, artist_type: ArtistType): self._artist_type = artist_type + def get_render_mode(self) -> RenderMode: + """ + Get the cache state of the artist. + + Returns: + RenderMode: The current cache state of the artist. + """ + return self._render_mode + + def set_render_mode(self, render_mode: RenderMode): + """ + Set the cache state of the artist. + + Args: + render_mode (RenderMode): The cache state to set. + """ + self._render_mode = render_mode + def draw(self): try: self._drawer(self._ctx, self.data) diff --git a/gamms/VisualizationEngine/builtin_artists.py b/gamms/VisualizationEngine/builtin_artists.py index 8ec9dea..12a6931 100644 --- a/gamms/VisualizationEngine/builtin_artists.py +++ b/gamms/VisualizationEngine/builtin_artists.py @@ -45,11 +45,13 @@ class AgentData: name (str): The name of the agent. color (ColorType): The color of the agent. size (float): The size of the agent. + image (Optional[object]): The image of the agent, if any. current_position (Optional[Tuple[float, float]]): The current position of the agent. """ name: str color: ColorType size: float + image: Optional[object] = field(default=None, init=False) current_position: Optional[Tuple[float, float]] = field(default=None, init=False) diff --git a/gamms/VisualizationEngine/default_drawers.py b/gamms/VisualizationEngine/default_drawers.py index 5614fd5..acb8dfc 100644 --- a/gamms/VisualizationEngine/default_drawers.py +++ b/gamms/VisualizationEngine/default_drawers.py @@ -1,5 +1,10 @@ from gamms.AgentEngine.agent_engine import AerialAgent -from gamms.VisualizationEngine import Color +from gamms.VisualizationEngine import ( + Color, + SHORT_EDGE_PIXEL_THRESHOLD, + SKIP_EDGE_PIXEL_THRESHOLD, + SKIP_NODE_PIXEL_THRESHOLD, +) from gamms.VisualizationEngine.builtin_artists import AgentData, GraphData from gamms.typing import IContext, OSMEdge, Node, ColorType, AgentType @@ -7,7 +12,6 @@ import math - def render_circle(ctx: IContext, data: Dict[str, Any]): """ Render a circle at the specified position with the specified radius and color. @@ -56,6 +60,7 @@ def render_agent(ctx: IContext, data: Dict[str, Any]): agent = ctx.agent.get_agent(agent_data.name) waiting_simulation = data.get('_waiting_simulation', False) + image = agent_data.image if agent.type == AgentType.BASIC: target_node = ctx.graph.graph.get_node(agent.current_node_id) @@ -63,23 +68,63 @@ def render_agent(ctx: IContext, data: Dict[str, Any]): prev_node = ctx.graph.graph.get_node(agent.prev_node_id) prev_position = (prev_node.x, prev_node.y) target_position = (target_node.x, target_node.y) - current_edge = None - for edge_id in ctx.graph.graph.get_edges(): - edge = ctx.graph.graph.get_edge(edge_id) - if edge.source == agent.prev_node_id and edge.target == agent.current_node_id: - current_edge = edge - alpha = cast(float, data.get('_alpha')) - if current_edge is not None: - point = current_edge.linestring.interpolate(alpha, True) - position = (point.x, point.y) - else: - position = (prev_position[0] + alpha * (target_position[0] - prev_position[0]), - prev_position[1] + alpha * (target_position[1] - prev_position[1])) - agent_data.current_position = position + viewport = ctx.visual.get_viewport() + if viewport is None: + return + + _, _, _, _, scale = viewport + dx = target_position[0] - prev_position[0] + dy = target_position[1] - prev_position[1] + d_sq = dx * dx + dy * dy + short_sq = _pixel_thresh_sq(SHORT_EDGE_PIXEL_THRESHOLD, scale) + skip_sq = _pixel_thresh_sq(SKIP_EDGE_PIXEL_THRESHOLD, scale) + + if skip_sq > 0.0 and d_sq <= skip_sq: + position = ((1 - alpha) * prev_position[0] + alpha * target_position[0], + (1 - alpha) * prev_position[1] + alpha * target_position[1]) + elif short_sq > 0.0 and d_sq <= short_sq: + position = ((1 - alpha) * prev_position[0] + alpha * target_position[0], + (1 - alpha) * prev_position[1] + alpha * target_position[1]) + else: + current_edge = None + cache_key = (agent.prev_node_id, agent.current_node_id) + cached_edge_entry = data.get('_current_edge_cache') + if cached_edge_entry is not None: + cached_edge_key, cached_edge = cached_edge_entry + if cached_edge_key == cache_key: + current_edge = cached_edge + + if current_edge is None: + neighbors = set(ctx.graph.graph.get_neighbors(agent.prev_node_id)) + if agent.current_node_id in neighbors: + midpoint_x = (prev_position[0] + target_position[0]) / 2 + midpoint_y = (prev_position[1] + target_position[1]) / 2 + max_dist = math.sqrt((target_position[0] - prev_position[0])**2 + + (target_position[1] - prev_position[1])**2) / 2 + 1.0 + for edge_id in ctx.graph.graph.get_edges(max_dist, midpoint_x, midpoint_y): + edge = ctx.graph.graph.get_edge(edge_id) + if edge.source == agent.prev_node_id and edge.target == agent.current_node_id: + current_edge = edge + data['_current_edge_cache'] = (cache_key, edge) + break + + if current_edge is not None: + point = current_edge.linestring.interpolate(alpha, True) + position = (point.x, point.y) + else: + position = ((1 - alpha) * prev_position[0] + alpha * target_position[0], + (1 - alpha) * prev_position[1] + alpha * target_position[1]) else: position = (target_node.x, target_node.y) + data.pop('_current_edge_cache', None) + + agent_data.current_position = position + + if image is not None: + ctx.visual.render_image(position[0], position[1], image, size) + return # Draw each agent as a triangle at its current position angle = math.radians(45) @@ -108,6 +153,10 @@ def render_agent(ctx: IContext, data: Dict[str, Any]): w = quat[0] angle = math.atan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z **2)) + if image is not None: + ctx.visual.render_image(position[0], position[1], image, size, angle=angle) + return + render_aerial_agent(ctx, position, angle, size, color) else: @@ -142,6 +191,23 @@ def render_aerial_agent(ctx: IContext, position: tuple[float, float], angle: flo ctx.visual.render_polygon(points, color) +def _pixel_thresh_sq(pixel_thresh: float, scale: float) -> float: + """ + Return the squared world-space distance that corresponds to a given number of screen pixels at the current camera zoom. + + Args: + pixel_thresh (float): Length in screen pixels to convert. + scale (float): Pixels-per-world-unit factor from the current viewport. + + Returns: + float: The squared world-space distance. Returns 0.0 if scale is non-positive. + """ + if scale <= 0: + return 0.0 + thresh_world = pixel_thresh / scale + return thresh_world * thresh_world + + def render_graph(ctx: IContext, data: Dict[str, Any]): """ Render the graph by drawing its nodes and edges on the screen. This is the default rendering method for graphs. @@ -156,13 +222,58 @@ def render_graph(ctx: IContext, data: Dict[str, Any]): edge_color = graph_data.edge_color draw_id = graph_data.draw_id - for edge_id in ctx.graph.graph.get_edges(): - edge = ctx.graph.graph.get_edge(edge_id) - _render_graph_edge(ctx, graph_data, edge, edge_color) - - for node_id in ctx.graph.graph.get_nodes(): - node = ctx.graph.graph.get_node(node_id) - _render_graph_node(ctx, node, node_color, node_size, draw_id) + graph = ctx.graph.graph + + viewport = ctx.visual.get_viewport() + if viewport is None: + return + left, right, top, bottom, scale = viewport + + x = (right + left) / 2 + y = (top + bottom) / 2 + d = max(right - left, bottom - top) + + short_sq = _pixel_thresh_sq(SHORT_EDGE_PIXEL_THRESHOLD, scale) + skip_sq = _pixel_thresh_sq(SKIP_EDGE_PIXEL_THRESHOLD, scale) + + for edge_id in graph.get_edges(d=d, x=x, y=y): + edge = graph.get_edge(edge_id) + _render_graph_edge(ctx, graph_data, edge, edge_color, short_sq, skip_sq) + + node_pixel_radius = node_size * scale + if node_pixel_radius >= SKIP_NODE_PIXEL_THRESHOLD: + for node_id in graph.get_nodes(d=d, x=x, y=y): + node = graph.get_node(node_id) + _render_graph_node(ctx, node, node_color, node_size, draw_id) + + +def render_obstacles(ctx: IContext, data: Dict[str, Any]): + boundary_thickness = cast(float, data.get('boundary_thickness', 1.0)) + color_code = cast(Dict[int, ColorType], data.get('color_map', {})) + viewport = ctx.visual.get_viewport() + if viewport is None: + return + + left, right, top, bottom, scale = viewport + + x = (right + left) / 2 + y = (top + bottom) / 2 + d = max(right - left, bottom - top) + + skip_sq = _pixel_thresh_sq(SKIP_EDGE_PIXEL_THRESHOLD, scale) + + for face_id in ctx.graph.get_obstacle_faces(d=d, x=x, y=y): + face = ctx.graph.get_obstacle_face(face_id) + color = color_code.get(cast(int, face.type), Color.Gray) + (x1, y1) = max(face.tr[0], face.br[0]), max(face.tr[1], face.br[1]) + (x2, y2) = min(face.tl[0], face.bl[0]), min(face.tl[1], face.bl[1]) + + if skip_sq > 0.0 and (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) <= skip_sq: + continue + + # Draw a line (x1, y1) to (x2, y2) with thickness scaled by the current zoom level + + ctx.visual.render_line(x1, y1, x2, y2, color, max(1, int(boundary_thickness * scale)), perform_culling_test=False, is_aa=False) def render_input_overlay(ctx: IContext, data: Dict[str, Any]): """ @@ -195,8 +306,15 @@ def render_input_overlay(ctx: IContext, data: Dict[str, Any]): draw_id = graph_data.draw_id target_node_id_set = set(input_options.values()) - for node in target_node_id_set: - _render_graph_node(ctx, graph.get_node(node), node_color, node_size, draw_id) + viewport = ctx.visual.get_viewport() + if viewport is None: + return + _, _, _, _, scale = viewport + + node_pixel_radius = node_size * scale + if node_pixel_radius >= SKIP_NODE_PIXEL_THRESHOLD: + for node in target_node_id_set: + _render_graph_node(ctx, graph.get_node(node), node_color, node_size, draw_id) active_edges: List[OSMEdge] = [] for edge_id in graph.get_edges(): @@ -204,26 +322,56 @@ def render_input_overlay(ctx: IContext, data: Dict[str, Any]): if edge.source == current_waiting_agent.current_node_id and edge.target in target_node_id_set: active_edges.append(edge) + short_sq = _pixel_thresh_sq(SHORT_EDGE_PIXEL_THRESHOLD, scale) + skip_sq = _pixel_thresh_sq(SKIP_EDGE_PIXEL_THRESHOLD, scale) for edge in active_edges: - _render_graph_edge(ctx, graph_data, edge, edge_color) + _render_graph_edge(ctx, graph_data, edge, edge_color, short_sq, skip_sq) + +def _render_graph_edge(ctx: IContext, graph_data: GraphData, edge: OSMEdge, color: ColorType, + short_edge_thresh_sq: float = 0.0, + skip_edge_thresh_sq: float = 0.0): + """ + Draw an edge as a curve or straight line based on the linestring. + + The squared world-space distance between the edge's endpoints drives + three mutually exclusive paths: -def _render_graph_edge(ctx: IContext, graph_data: GraphData, edge: OSMEdge, color: ColorType): - """Draw an edge as a curve or straight line based on the linestring.""" + * endpoints within sqrt(skip_edge_thresh_sq) world units -> drop + the edge entirely (sub-pixel on screen). + * linestring edge within sqrt(short_edge_thresh_sq) world units -> + draw as a single straight segment, skipping Shapely deserialization + and the multi-segment renderer call. + * otherwise -> draw the full linestring. + """ source = ctx.graph.graph.get_node(edge.source) target = ctx.graph.graph.get_node(edge.target) - if edge.linestring: - edge_line_points = graph_data.edge_line_points - if edge.id not in edge_line_points: - # linestring[1:-1] - linestring = ([(source.x, source.y)] + [(x, y) for (x, y) in edge.linestring.coords] + - [(target.x, target.y)]) - edge_line_points[edge.id] = linestring + dx = target.x - source.x + dy = target.y - source.y + d_sq = dx * dx + dy * dy - line_points = edge_line_points[edge.id] - ctx.visual.render_linestring(line_points, color, is_aa=True, perform_culling_test=False) - else: - ctx.visual.render_line(source.x, source.y, target.x, target.y, color, 2, perform_culling_test=False, is_aa=False) + if skip_edge_thresh_sq > 0.0 and d_sq <= skip_edge_thresh_sq: + return + + if short_edge_thresh_sq > 0.0 and d_sq <= short_edge_thresh_sq: + ctx.visual.render_line(source.x, source.y, target.x, target.y, color, 2, + perform_culling_test=False, is_aa=False) + return + + edge_line_points = graph_data.edge_line_points + line_points = edge_line_points.get(edge.id) + + if line_points is None: + linestring = edge.linestring + if not linestring: + ctx.visual.render_line(source.x, source.y, target.x, target.y, color, 2, + perform_culling_test=False, is_aa=False) + return + line_points = ([(source.x, source.y)] + [(x, y) for (x, y) in linestring.coords] + + [(target.x, target.y)]) + edge_line_points[edge.id] = line_points + + ctx.visual.render_linestring(line_points, color, is_aa=True, perform_culling_test=False) def _render_graph_node(ctx: IContext, node: Node, color: ColorType, radius: float, draw_id: bool): @@ -269,11 +417,30 @@ def render_map_sensor(ctx: IContext, data: Dict[str, Any]): edge_color = data.get('edge_color', Color.Cyan) sensed_edges = sensor_data.get('edges', []) - + + viewport = ctx.visual.get_viewport() + if viewport is None: + return + _, _, _, _, scale = viewport + short_sq = _pixel_thresh_sq(SHORT_EDGE_PIXEL_THRESHOLD, scale) + skip_sq = _pixel_thresh_sq(SKIP_EDGE_PIXEL_THRESHOLD, scale) + for edge in sensed_edges: source = ctx.graph.graph.get_node(edge.source) target = ctx.graph.graph.get_node(edge.target) + dx = target.x - source.x + dy = target.y - source.y + d_sq = dx * dx + dy * dy + + if skip_sq > 0.0 and d_sq <= skip_sq: + continue + + if short_sq > 0.0 and d_sq <= short_sq: + ctx.visual.render_line(source.x, source.y, target.x, target.y, edge_color, 4, + perform_culling_test=False, is_aa=False) + continue + if edge.linestring: # linestring[1:-1] line_points = ([(source.x, source.y)] + [(x, y) for (x, y) in edge.linestring.coords] + diff --git a/gamms/VisualizationEngine/no_engine.py b/gamms/VisualizationEngine/no_engine.py index d5b8f0e..8674c00 100644 --- a/gamms/VisualizationEngine/no_engine.py +++ b/gamms/VisualizationEngine/no_engine.py @@ -9,7 +9,7 @@ from gamms.VisualizationEngine.artist import Artist from gamms.VisualizationEngine import Color -from typing import Dict, Any, List, Tuple, Callable, cast, Union +from typing import Dict, Any, List, Tuple, Callable, Optional, cast, Union class NoEngine(IVisualizationEngine): def __init__(self, ctx: IContext, **kwargs: Dict[str, Any]) -> None: @@ -19,6 +19,10 @@ def set_graph_visual(self, **kwargs: Dict[str, Any]) -> IArtist: dummy = cast(Callable[[IContext, Dict[str, Any]], None], lambda ctx, data: None) return Artist(self.ctx , dummy, layer=10) + def set_obstacle_visual(self, **kwargs: Dict[str, Any]) -> IArtist: + dummy = cast(Callable[[IContext, Dict[str, Any]], None], lambda ctx, data: None) + return Artist(self.ctx , dummy, layer=5) + def set_agent_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: dummy = cast(Callable[[IContext, Dict[str, Any]], None], lambda ctx, data: None) return Artist(self.ctx , dummy, layer=20) @@ -76,6 +80,13 @@ def render_linestring(self, points: List[Tuple[float, float]], color: ColorType def render_polygon(self, points: List[Tuple[float, float]], color: ColorType = Color.Black, width: int=0, perform_culling_test: bool=True): return + + def render_image(self, x: float, y: float, image: Any, size: float, angle: float = 0.0, + perform_culling_test: bool = True): + return def render_layer(self, layer_id: int) -> None: - return \ No newline at end of file + return + + def get_viewport(self) -> Optional[Tuple[float, float, float, float, float]]: + return None \ No newline at end of file diff --git a/gamms/VisualizationEngine/pygame_engine.py b/gamms/VisualizationEngine/pygame_engine.py index 74abe92..ce87936 100644 --- a/gamms/VisualizationEngine/pygame_engine.py +++ b/gamms/VisualizationEngine/pygame_engine.py @@ -1,13 +1,20 @@ from gamms.AgentEngine.agent_engine import AerialAgent -from gamms.VisualizationEngine import Color, Space, Shape, Artist, lazy +from gamms.VisualizationEngine import ( + Color, + Space, + Shape, + Artist, + lazy, + CACHE_ZOOM_MAX, +) from gamms.VisualizationEngine.render_manager import RenderManager from gamms.VisualizationEngine.builtin_artists import AgentData, GraphData from gamms.VisualizationEngine.default_drawers import ( - render_circle, render_rectangle, render_agent, render_graph, render_neighbor_sensor, render_map_sensor, render_agent_sensor, render_input_overlay, - render_aerial_agent_sensor, + render_aerial_agent_sensor, render_obstacles ) +from ..osm_constants import COLOR_TYPES from gamms.typing import ( IVisualizationEngine, IArtist, @@ -18,7 +25,19 @@ ColorType, AgentType ) -from typing import Dict, Any, List, Tuple, Union, cast, Optional +from typing import Dict, Any, List, NamedTuple, Tuple, Union, cast, Optional, Iterator, Set +from pathlib import Path +import math + +class _LayerCache(NamedTuple): + surface: Any + artist_names: Tuple[str, ...] + camera_x: float + camera_y: float + camera_size: float + screen_width: int + screen_height: int + class PygameVisualizationEngine(IVisualizationEngine): def __init__(self, ctx: IContext, width: int = 1280, height: int = 720, simulation_time_constant: float = 2.0, **kwargs : Dict[str, Any]): @@ -42,21 +61,17 @@ def __init__(self, ctx: IContext, width: int = 1280, height: int = 720, simulati self._simulation_time = 0 self._will_quit = False self._render_manager = RenderManager(ctx, 0, 0, 15, width, height) - self._surface_dict : Dict[int, self._pygame.Surface ] = {} - self._agent_artists: Dict[str, IArtist] = {} - self._graph_artists: Dict[str, IArtist] = {} + self._render_manager.set_cached_layer_handler(self._blit_layer_cache) + self._render_surface = self._pygame.Surface((width, height), self._pygame.SRCALPHA) + self._layer_caches: Dict[int, _LayerCache] = {} + self._building_layer_surface: Optional[Any] = None + self._dynamic_artists: Dict[str, IArtist] = {} + self._dynamic_agent_artist_names: Set[str] = set() input_overlay_args = kwargs.get('input_overlay', {}) self._input_overlay_artist = self._set_input_overlay_artist(input_overlay_args) def create_layer(self, layer_id: int, width : int, height : int) -> int: - if layer_id not in self._surface_dict: - surface = self._pygame.Surface((width, height), self._pygame.SRCALPHA) - self._surface_dict[layer_id] = surface - - # Order layers by ascending order - self._surface_dict = {id: self._surface_dict[id] for id in sorted(self._surface_dict.keys())} - return layer_id def set_graph_visual(self, **kwargs: Dict[str, Any]) -> IArtist: @@ -88,14 +103,26 @@ def set_graph_visual(self, **kwargs: Dict[str, Any]) -> IArtist: artist = Artist(self.ctx, render_graph, 10) artist.data['graph_data'] = graph_data - artist.set_will_draw(False) - artist.set_artist_type(ArtistType.GRAPH) + artist.set_artist_type(ArtistType.STATIC) #Add data for node ID and Color self.add_artist('graph', artist) - # Trigger the redraw of the graph artists after it has been added - self._redraw_graph_artists() + return artist + + def set_obstacle_visual(self, **kwargs: Dict[str, Any]) -> IArtist: + boundary_thickness = cast(float, kwargs.get('boundary_thickness', 1.0)) + color_code = cast(Dict[int, ColorType], {k:tuple(int(color[i:i+2], 16) for i in (1, 3, 5)) for k, color in COLOR_TYPES.items()}) + color_code.update( + cast(Dict[int, ColorType], kwargs.get('color_map', {})) + ) + + artist = Artist(self.ctx, render_obstacles, 5) + artist.data['boundary_thickness'] = boundary_thickness + artist.data['color_map'] = color_code + artist.set_artist_type(ArtistType.STATIC) + + self.add_artist('obstacles', artist) return artist @@ -111,28 +138,41 @@ def _set_input_overlay_artist(self, args: Dict[str, Any]) -> IArtist: artist.data['_waiting_user_input'] = False artist.data['graph_data'] = graph_data artist.set_visible(False) + artist.set_artist_type(ArtistType.DYNAMIC) self.add_artist('input_overlay', artist) return artist def set_agent_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: - agent_data = AgentData( name=name, color=cast(ColorType, kwargs.get('color', Color.Black)), size=cast(int,kwargs.get('size', 8)) ) + agent_data.image = self._load_agent_image(kwargs.get('image_path')) artist = Artist(self.ctx, render_agent, 20) artist.data['agent_data'] = agent_data - artist.set_artist_type(ArtistType.AGENT) + artist.set_artist_type(ArtistType.DYNAMIC) artist.data['_alpha'] = 1.0 self.add_artist(name, artist) return artist + def _load_agent_image(self, image_path: str): + if image_path is None: + return None + + path = Path(str(image_path)) + if not path.exists(): + raise FileNotFoundError(f"Agent image not found: {path}") + + image = self._pygame.image.load(str(path)) + + return image.convert_alpha() + def set_sensor_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: sensor = self.ctx.sensor.get_sensor(name) sensor_type = sensor.type @@ -145,15 +185,17 @@ def set_sensor_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: drawer = render_neighbor_sensor data['color'] = kwargs.pop('color', Color.Cyan) data['size'] = kwargs.pop('size', 8) - elif sensor_type in (SensorType.MAP, SensorType.RANGE, SensorType.ARC, SensorType.AERIAL): + elif sensor_type in (SensorType.MAP, SensorType.RANGE, SensorType.ARC, SensorType.AERIAL, + SensorType.OCCLUDED_MAP, SensorType.OCCLUDED_AERIAL): drawer = render_map_sensor data['node_color'] = kwargs.pop('node_color', Color.Cyan) data['edge_color'] = kwargs.pop('edge_color', Color.Cyan) - elif sensor_type in (SensorType.AGENT, SensorType.AGENT_RANGE, SensorType.AGENT_ARC): + elif sensor_type in (SensorType.AGENT, SensorType.AGENT_RANGE, SensorType.AGENT_ARC, + SensorType.OCCLUDED_AGENT): drawer = render_agent_sensor data['color'] = kwargs.pop('color', Color.Cyan) data['size'] = kwargs.pop('size', 8) - elif sensor_type == SensorType.AERIAL_AGENT: + elif sensor_type in (SensorType.AERIAL_AGENT, SensorType.OCCLUDED_AERIAL_AGENT): drawer = render_aerial_agent_sensor data['color'] = kwargs.pop('color', Color.Cyan) data['size'] = kwargs.pop('size', 8) @@ -163,6 +205,7 @@ def set_sensor_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: layer = cast(int, kwargs.pop('layer', 30)) artist = Artist(self.ctx, drawer, layer) artist.data.update(data) + artist.set_artist_type(ArtistType.DYNAMIC) self.add_artist(f'sensor_{name}', artist) @@ -187,16 +230,26 @@ def add_artist(self, name: str, artist: Union[IArtist, Dict[str, Any]]) -> IArti artist_to_add = Artist(self.ctx, drawer, layer) artist_to_add.data = artist - layer = artist_to_add.get_layer() - if artist_to_add.get_artist_type() == ArtistType.AGENT: - self._agent_artists[name] = artist_to_add - elif artist_to_add.get_artist_type() == ArtistType.GRAPH: - self._graph_artists[name] = artist_to_add + if artist_to_add.get_artist_type() == ArtistType.STATIC: + self._dynamic_artists.pop(name, None) + self._dynamic_agent_artist_names.discard(name) + self._layer_caches.pop(artist_to_add.get_layer(), None) + elif artist_to_add.get_artist_type() == ArtistType.DYNAMIC: + self._dynamic_artists[name] = artist_to_add + if 'agent_data' in artist_to_add.data: + self._dynamic_agent_artist_names.add(name) + else: + self._dynamic_agent_artist_names.discard(name) self._render_manager.add_artist(name, artist_to_add) return artist_to_add def remove_artist(self, name: str): + artist = self._render_manager.get_artist(name) + if artist is not None and artist.get_artist_type() == ArtistType.STATIC: + self._layer_caches.pop(artist.get_layer(), None) + self._dynamic_artists.pop(name, None) + self._dynamic_agent_artist_names.discard(name) self._render_manager.remove_artist(name) def handle_input(self): @@ -204,19 +257,15 @@ def handle_input(self): scroll_speed = self._render_manager.camera_size / 2 if pressed_keys[self._pygame.K_a] or pressed_keys[self._pygame.K_LEFT]: self._render_manager.camera_x -= (scroll_speed * self._clock.get_time() / 1000) - self._redraw_graph_artists() if pressed_keys[self._pygame.K_d] or pressed_keys[self._pygame.K_RIGHT]: self._render_manager.camera_x += (scroll_speed * self._clock.get_time() / 1000) - self._redraw_graph_artists() if pressed_keys[self._pygame.K_w] or pressed_keys[self._pygame.K_UP]: self._render_manager.camera_y += (scroll_speed * self._clock.get_time() / 1000) - self._redraw_graph_artists() if pressed_keys[self._pygame.K_s] or pressed_keys[self._pygame.K_DOWN]: self._render_manager.camera_y -= (scroll_speed * self._clock.get_time() / 1000) - self._redraw_graph_artists() for event in self._pygame.event.get(): if event.type == self._pygame.MOUSEWHEEL: @@ -225,21 +274,18 @@ def handle_input(self): self._render_manager.camera_size /= 1.05 else: self._render_manager.camera_size *= 1.05 - - self._redraw_graph_artists() if event.type == self._pygame.QUIT: self._will_quit = True self._input_option_result = -1 self._input_position_result = -1 if event.type == self._pygame.VIDEORESIZE: - self._render_manager.screen_width = event.w - self._render_manager.screen_height = event.h - self._screen = self._pygame.display.set_mode((event.w, event.h), self._pygame.RESIZABLE) - for layer_id in self._surface_dict.keys(): - self._surface_dict[layer_id] = self._pygame.Surface((event.w, event.h), self._pygame.SRCALPHA) - - self._redraw_graph_artists() + event_w = max(1, event.w) + event_h = max(1, event.h) + self._render_manager.set_screen_size(event_w, event_h) + self._screen = self._pygame.display.set_mode((event_w, event_h), self._pygame.RESIZABLE) + self._render_surface = self._pygame.Surface((event_w, event_h), self._pygame.SRCALPHA) + self._layer_caches.clear() if self._waiting_user_input: if self._waiting_agent_name is None: @@ -276,16 +322,18 @@ def handle_tick(self): self._simulation_time += self._clock.get_time() / 1000 alpha = self._simulation_time / self._sim_time_constant alpha = self._pygame.math.clamp(alpha, 0, 1) - for agent_artist in self._agent_artists.values(): - agent_artist.data['_alpha'] = alpha + for artist in self._dynamic_artists.values(): + artist.data['_alpha'] = alpha def handle_single_draw(self): self._screen.fill(Color.White) + self._render_surface.fill((0, 0, 0, 0)) # Note: Draw in layer order of back layer -> front layer # self._draw_grid() - + self._render_manager.handle_render() + self._screen.blit(self._render_surface, (0, 0)) self.draw_input_overlay() self.draw_hud() @@ -341,26 +389,169 @@ def _render_text_internal(self, text: str, x: float, y: float, coord_space: Spac else: raise ValueError("Invalid coord_space value. Must be one of the values in the Space enum.") - def _redraw_graph_artists(self): - for artist_name, graph_artist in self._graph_artists.items(): - self.clear_layer(graph_artist.get_layer()) - self._render_manager.render_single_artist(artist_name) + def _rebuild_layer_cache(self, layer: int, names: List[str]) -> None: + rm = self._render_manager + surface = self._pygame.Surface( + (rm.screen_width, rm.screen_height), self._pygame.SRCALPHA + ) + surface.fill((0, 0, 0, 0)) + + self._building_layer_surface = surface + try: + for name in names: + self._render_manager.render_single_artist(name) + finally: + self._building_layer_surface = None + + self._layer_caches[layer] = _LayerCache( + surface=surface, + artist_names=tuple(names), + camera_x=rm.camera_x, + camera_y=rm.camera_y, + camera_size=rm.camera_size, + screen_width=rm.screen_width, + screen_height=rm.screen_height, + ) + + def _blit_layer_cache(self, layer: int, names: List[str]) -> None: + rm = self._render_manager + cache = self._layer_caches.get(layer) + expected_names = tuple(names) + + if (cache is None + or cache.artist_names != expected_names + or cache.screen_width != rm.screen_width + or cache.screen_height != rm.screen_height): + self._rebuild_layer_cache(layer, names) + cache = self._layer_caches[layer] + + zoom_ratio = cache.camera_size / rm.camera_size + if zoom_ratio < 1.0 or zoom_ratio > CACHE_ZOOM_MAX: + self._rebuild_layer_cache(layer, names) + cache = self._layer_caches[layer] + zoom_ratio = 1.0 + + dx_px = int(round(rm.world_to_screen_scale(rm.camera_x - cache.camera_x))) + dy_px = int(round(rm.world_to_screen_scale(rm.camera_y - cache.camera_y))) + + if (abs(dx_px) >= rm.screen_width // 4 + or abs(dy_px) >= rm.screen_height // 4): + self._rebuild_layer_cache(layer, names) + cache = self._layer_caches[layer] + dx_px = 0 + dy_px = 0 + zoom_ratio = 1.0 + + if zoom_ratio == 1.0: + self._render_surface.blit(cache.surface, (-dx_px, dy_px)) + cover_x0 = -dx_px + cover_y0 = dy_px + cover_w = rm.screen_width + cover_h = rm.screen_height + else: + W = rm.screen_width + H = rm.screen_height + scaled_w = max(1, int(round(W * zoom_ratio))) + scaled_h = max(1, int(round(H * zoom_ratio))) + scaled = self._pygame.transform.scale( + cache.surface, (scaled_w, scaled_h) + ) + offset_x = -dx_px + (W - scaled_w) // 2 + offset_y = dy_px + (H - scaled_h) // 2 + self._render_surface.blit(scaled, (offset_x, offset_y)) + cover_x0 = offset_x + cover_y0 = offset_y + cover_w = scaled_w + cover_h = scaled_h + + self._redraw_layer_strips(names, cover_x0, cover_y0, cover_w, cover_h) + + def _redraw_layer_strips(self, names: List[str], cover_x0: int, cover_y0: int, cover_w: int, cover_h: int) -> None: + rm = self._render_manager + W = rm.screen_width + H = rm.screen_height + + cx_min = max(0, cover_x0) + cy_min = max(0, cover_y0) + cx_max = min(W, cover_x0 + cover_w) + cy_max = min(H, cover_y0 + cover_h) + + strips: List[Tuple[int, int, int, int]] = [] + if cy_min > 0: + strips.append((0, 0, W, cy_min)) + if cy_max < H: + strips.append((0, cy_max, W, H - cy_max)) + middle_h = max(0, cy_max - cy_min) + if middle_h > 0: + if cx_min > 0: + strips.append((0, cy_min, cx_min, middle_h)) + if cx_max < W: + strips.append((cx_max, cy_min, W - cx_max, middle_h)) + + if not strips: + return + + for sx, sy, sw, sh in strips: + if sw <= 0 or sh <= 0: + continue + wl, wb = rm.screen_to_world(sx, sy) + wr, wt = rm.screen_to_world(sx + sw, sy + sh) + rm.set_culling_bounds(wl, wr, wt, wb) + self._render_surface.set_clip(self._pygame.Rect(sx, sy, sw, sh)) + try: + for name in names: + self._render_manager.render_single_artist(name) + finally: + self._render_surface.set_clip(None) + rm.reset_culling_bounds() + + def _iter_agent_artists(self) -> Iterator[IArtist]: + stale_names: List[str] = [] + for name in self._dynamic_agent_artist_names: + artist = self._dynamic_artists.get(name) + if artist is None or 'agent_data' not in artist.data: + stale_names.append(name) + continue + + yield artist + + for name in stale_names: + self._dynamic_agent_artist_names.discard(name) + + def _get_agent_artist(self, agent_name: str) -> IArtist: + if agent_name not in self._dynamic_agent_artist_names: + raise KeyError(f"Agent artist {agent_name} not found") + + artist = self._dynamic_artists.get(agent_name) + if artist is None or 'agent_data' not in artist.data: + self._dynamic_agent_artist_names.discard(agent_name) + raise KeyError(f"Agent artist {agent_name} not found") + + return artist def _toggle_waiting_simulation(self, waiting_simulation: bool): self._waiting_simulation = waiting_simulation - for agent_artist in self._agent_artists.values(): - agent_artist.data['_alpha'] = 0.0 + for agent_artist in self._iter_agent_artists(): agent_artist.data['_waiting_simulation'] = waiting_simulation + for artist in self._dynamic_artists.values(): + artist.data['_alpha'] = 0.0 def _toggle_waiting_user_input(self, waiting_user_input: bool): self._waiting_user_input = waiting_user_input self._input_overlay_artist.data['_waiting_user_input'] = waiting_user_input - def _get_target_surface(self, layer: int): - if layer >= 0: - return self._surface_dict.get(layer, self._screen) - else: - return self._screen + def _get_target_surface(self): + if self._building_layer_surface is not None: + return self._building_layer_surface + return self._render_surface + + def get_viewport(self) -> Optional[Tuple[float, float, float, float, float]]: + rm = self._render_manager + if rm.screen_width <= 0 or rm.screen_height <= 0 or rm.camera_size <= 0: + self.ctx.logger.warning("Invalid viewport state") + return None + scale = rm.world_to_screen_scale(1.0) + return (rm.bound_left, rm.bound_right, rm.bound_top, rm.bound_bottom, scale) def render_text(self, text: str, x: float, y: float, color: ColorType = Color.Black, perform_culling_test: bool=True, font_size: Optional[int]=None): if font_size is not None: @@ -379,8 +570,7 @@ def render_text(self, text: str, x: float, y: float, color: ColorType = Color.Bl if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + surface = self._get_target_surface() surface.blit(text_surface, text_rect) def render_rectangle(self, x: float, y: float, width: float, height: float, color: ColorType = Color.Black, @@ -393,8 +583,7 @@ def render_rectangle(self, x: float, y: float, width: float, height: float, colo if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + surface = self._get_target_surface() self._pygame.draw.rect(surface, color, self._pygame.Rect(x, y, width, height)) def render_circle(self, x: float, y: float, radius: float, color: ColorType = Color.Black, width: int = 0, @@ -409,9 +598,8 @@ def render_circle(self, x: float, y: float, radius: float, color: ColorType = Co if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + + surface = self._get_target_surface() self._pygame.draw.circle(surface, color, (x, y), radius, width) def render_line(self, start_x: float, start_y: float, end_x: float, end_y: float, color: ColorType = Color.Black, @@ -425,8 +613,7 @@ def render_line(self, start_x: float, start_y: float, end_x: float, end_y: float if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + surface = self._get_target_surface() if is_aa: self._pygame.draw.aaline(surface, color, (start_x, start_y), (end_x, end_y)) else: @@ -442,8 +629,7 @@ def render_linestring(self, points: List[Tuple[float, float]], color: ColorType if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + surface = self._get_target_surface() if is_aa: self._pygame.draw.aalines(surface, color, closed, points) else: @@ -459,22 +645,51 @@ def render_polygon(self, points: List[Tuple[float, float]], color: ColorType = C if self._render_manager.current_drawing_artist is None: raise ValueError("No current drawing artist set.") - layer = self._render_manager.current_drawing_artist.get_layer() - surface = self._get_target_surface(layer) + surface = self._get_target_surface() self._pygame.draw.polygon(surface, color, points, width) + def render_image(self, x: float, y: float, image: Any, size: float, angle: float = 0.0, + perform_culling_test: bool = True): + if image is None: + return + + diameter = size * 2 + if perform_culling_test and self._render_manager.check_rectangle_culled(x, y, diameter, diameter): + return + + if self._render_manager.current_drawing_artist is None: + raise ValueError("No current drawing artist set.") + + base_image = image + if angle != 0.0: + base_image = self._pygame.transform.rotate(base_image, -math.degrees(angle)) + + target_width = max(1, int(round(self._render_manager.world_to_screen_scale(diameter)))) + image_rect = base_image.get_rect() + if image_rect.width <= 0 or image_rect.height <= 0: + return + + scale_factor = min(target_width / image_rect.width, target_width / image_rect.height) + scaled_size = ( + max(1, int(round(image_rect.width * scale_factor))), + max(1, int(round(image_rect.height * scale_factor))), + ) + scaled_image = self._pygame.transform.smoothscale(base_image, scaled_size) + + screen_x, screen_y = self._render_manager.world_to_screen(x, y) + draw_rect = scaled_image.get_rect(center=(screen_x, screen_y)) + + surface = self._get_target_surface() + surface.blit(scaled_image, draw_rect) + def clear_layer(self, layer_id: int): - if layer_id in self._surface_dict: - self._surface_dict[layer_id].fill((0, 0, 0, 0)) + return def fill_layer(self, layer_id: int, color: ColorType): - if layer_id in self._surface_dict: - self._surface_dict[layer_id].fill(color) + return def render_layer(self, layer_id: int): - if layer_id in self._surface_dict: - surface = self._surface_dict[layer_id] - self._screen.blit(surface, (0, 0)) + return def _draw_grid(self): x_min = self._render_manager.camera_x - self._render_manager.camera_size * 4 @@ -508,11 +723,11 @@ def human_input(self, agent_name: str, state: Dict[str, Any]) -> Union[int, Tupl prev_waiting_agent_name = self._waiting_agent_name if prev_waiting_agent_name is not None: - prev_waiting_agent_artist = self._agent_artists[prev_waiting_agent_name] + prev_waiting_agent_artist = self._get_agent_artist(prev_waiting_agent_name) prev_waiting_agent_artist.data['_is_waiting'] = False self._waiting_agent_name = agent_name - waiting_agent_artist = self._agent_artists[agent_name] + waiting_agent_artist = self._get_agent_artist(agent_name) waiting_agent_artist.data['_is_waiting'] = True waiting_agent = self.ctx.agent.get_agent(agent_name) @@ -526,7 +741,6 @@ def human_input(self, agent_name: str, state: Dict[str, Any]) -> Union[int, Tupl self._input_overlay_artist.data['_input_options'] = self._input_options self._input_overlay_artist.set_visible(True) - self._redraw_graph_artists() while self._waiting_user_input: # still need to update the render @@ -566,7 +780,7 @@ def human_input(self, agent_name: str, state: Dict[str, Any]) -> Union[int, Tupl raise RuntimeError(f"Unknown agent type {waiting_agent.type} for agent {agent_name}") def end_handle_human_input(self): - for agent_artist in self._agent_artists.values(): + for agent_artist in self._iter_agent_artists(): agent_artist.data['_is_waiting'] = False self._input_overlay_artist.data['_waiting_agent_name'] = None @@ -577,7 +791,6 @@ def end_handle_human_input(self): self._input_option_result = None self._input_position_result = None self._waiting_agent_name = None - self._redraw_graph_artists() def simulate(self): if self.ctx.record.record(): diff --git a/gamms/VisualizationEngine/render_manager.py b/gamms/VisualizationEngine/render_manager.py index e0f4565..db135b0 100644 --- a/gamms/VisualizationEngine/render_manager.py +++ b/gamms/VisualizationEngine/render_manager.py @@ -1,6 +1,7 @@ +from gamms.VisualizationEngine import RenderMode from gamms.typing import ArtistType, IContext, IArtist -from typing import Set, Dict, List, Optional, Tuple +from typing import Callable, Dict, List, Optional, Tuple class RenderManager: @@ -9,30 +10,54 @@ def __init__(self, ctx: IContext, camera_x: float, camera_y: float, camera_size: self._screen_width = screen_width self._screen_height = screen_height - self._aspect_ratio = self._screen_width / self._screen_height self._camera_x = int(camera_x) self._camera_y = int(camera_y) self._camera_size = camera_size - self._camera_size_y = camera_size / self.aspect_ratio - - self._update_bounds() + self._update_projection() self._artists: Dict[str, IArtist] = {} # This will call drawer on all artists in the respective layer self._layer_artists: Dict[int, List[str]] = {} - self._graph_layers: Set[int] = set() self._current_drawing_artist: Optional[IArtist] = None + self._cached_layer_handler: Optional[Callable[[int, List[str]], None]] = None self._default_origin = (0, 0) self._surface_size = 0 + def set_cached_layer_handler(self, handler: Optional[Callable[[int, List[str]], None]]) -> None: + """ + Register a callable invoked once per layer for cached static artists. + Passing None clears the hook. + + Args: + handler (Optional[Callable[[int, List[str]], None]]): Callable taking + the layer id and static artist names, or None to clear any hook. + """ + self._cached_layer_handler = handler + def _update_bounds(self): self._bound_left = -self.camera_size + self.camera_x self._bound_right = self.camera_size + self.camera_x self._bound_top = -self.camera_size_y + self.camera_y self._bound_bottom = self.camera_size_y + self.camera_y + def _update_projection(self): + self._screen_width = max(1, self._screen_width) + self._screen_height = max(1, self._screen_height) + self._aspect_ratio = self._screen_width / self._screen_height + self._camera_size_y = self._camera_size / self._aspect_ratio + self._update_bounds() + + def set_culling_bounds(self, left: float, right: float, top: float, bottom: float) -> None: + self._bound_left = left + self._bound_right = right + self._bound_top = top + self._bound_bottom = bottom + + def reset_culling_bounds(self) -> None: + self._update_bounds() + def set_origin(self, x: float, y: float, graph_width: float, graph_height: float): self.camera_x = int(x) self.camera_y = int(y) @@ -70,8 +95,7 @@ def camera_size(self): @camera_size.setter def camera_size(self, value: float): self._camera_size = value - self._camera_size_y = self.camera_size / self.aspect_ratio - self._update_bounds() + self._update_projection() @property def camera_size_y(self): @@ -90,7 +114,7 @@ def screen_width(self): @screen_width.setter def screen_width(self, value: int): self._screen_width = value - self._aspect_ratio = self._screen_width / self._screen_height + self._update_projection() @property def screen_height(self): @@ -99,7 +123,12 @@ def screen_height(self): @screen_height.setter def screen_height(self, value: int): self._screen_height = value - self._aspect_ratio = self._screen_width / self._screen_height + self._update_projection() + + def set_screen_size(self, width: int, height: int) -> None: + self._screen_width = width + self._screen_height = height + self._update_projection() @property def aspect_ratio(self): @@ -108,6 +137,22 @@ def aspect_ratio(self): @property def current_drawing_artist(self): return self._current_drawing_artist + + @property + def bound_left(self): + return self._bound_left + + @property + def bound_right(self): + return self._bound_right + + @property + def bound_top(self): + return self._bound_top + + @property + def bound_bottom(self): + return self._bound_bottom def world_to_screen_scale(self, world_size: float) -> float: """ @@ -208,9 +253,6 @@ def add_artist(self, name: str, artist: IArtist) -> None: else: self._layer_artists[artist.get_layer()].append(name) - if artist.get_artist_type() == ArtistType.GRAPH: - self._graph_layers.add(artist.get_layer()) - def remove_artist(self, name: str): """ Remove an artist from the render manager. @@ -226,18 +268,17 @@ def remove_artist(self, name: str): else: print(f"Warning: Artist {name} not found.") + def get_artist(self, name: str) -> Optional[IArtist]: + return self._artists.get(name) + def rebuild_artist_layer(self): self._layer_artists.clear() - self._graph_layers.clear() for name, artist in self._artists.items(): if artist.get_layer() not in self._layer_artists: self._layer_artists[artist.get_layer()] = [name] else: self._layer_artists[artist.get_layer()].append(name) - if artist.get_artist_type() == ArtistType.GRAPH: - self._graph_layers.add(artist.get_layer()) - self._layer_artists = {k: self._layer_artists[k] for k in sorted(self._layer_artists.keys())} def render_single_artist(self, artist_name: str): @@ -246,6 +287,7 @@ def render_single_artist(self, artist_name: str): self.ctx.logger.warning(f"Artist {artist_name} not found.") return + artist.set_render_mode(RenderMode.NON_CACHED) self._current_drawing_artist = artist artist.draw() self._current_drawing_artist = None @@ -262,19 +304,28 @@ def handle_render(self): for artist in self._artists.values(): artist.layer_dirty = False - rendered_layers: Set[int] = set() for layer, artist_name_list in self._layer_artists.items(): + cached_static_names: List[str] = [] + non_cached_artists: List[IArtist] = [] for artist_name in artist_name_list: artist = self._artists[artist_name] if not artist.get_visible(): continue - if not artist.get_will_draw(): - if artist.get_artist_type() == ArtistType.GRAPH and layer not in rendered_layers: - self.ctx.visual.render_layer(layer) - rendered_layers.add(layer) - continue + if (artist.get_artist_type() == ArtistType.STATIC + and self._cached_layer_handler is not None): + cached_static_names.append(artist_name) + else: + non_cached_artists.append(artist) + + if cached_static_names: + for artist_name in cached_static_names: + self._artists[artist_name].set_render_mode(RenderMode.CACHED) + assert self._cached_layer_handler is not None + self._cached_layer_handler(layer, cached_static_names) + for artist in non_cached_artists: + artist.set_render_mode(RenderMode.NON_CACHED) self._current_drawing_artist = artist artist.draw() self._current_drawing_artist = None \ No newline at end of file diff --git a/gamms/__init__.py b/gamms/__init__.py index 51e8e3a..7012a8a 100644 --- a/gamms/__init__.py +++ b/gamms/__init__.py @@ -2,8 +2,10 @@ import gamms.SensorEngine.sensor_engine as sensor import gamms.GraphEngine.graph_engine as graph import gamms.VisualizationEngine as visual +import gamms.MemoryEngine as memory from gamms.Recorder.recorder import Recorder from gamms.context import Context +from gamms.internal_context import InternalContext from enum import Enum from gamms.typing import logger @@ -34,6 +36,12 @@ def create_context( agent_engine = agent.AgentEngine(ctx) sensor_engine = sensor.SensorEngine(ctx) + memory_engine = memory.MemoryEngine() + ctx.internal_context = InternalContext( + compute_engine=None, + memory_engine=memory_engine, + message_engine=None, + ) ctx.agent_engine = agent_engine ctx.graph_engine = graph.GraphEngine(ctx, engine=graph_engine) ctx.visual_engine = visual_engine @@ -61,4 +69,4 @@ def create_context( ctx.set_alive() return ctx -__version__ = "0.2.7" \ No newline at end of file +__version__ = "1.0.0" diff --git a/gamms/internal_context.py b/gamms/internal_context.py index c96bcc3..48a789a 100644 --- a/gamms/internal_context.py +++ b/gamms/internal_context.py @@ -1,29 +1,35 @@ +from typing import Optional + from gamms.typing import IComputeEngine, IMemoryEngine, IMessageEngine, IInternalContext + class InternalContext(IInternalContext): def __init__( self, - compute_engine: IComputeEngine, - memory_engine: IMemoryEngine, - message_engine: IMessageEngine, - ) -> None: + compute_engine: Optional[IComputeEngine] = None, + memory_engine: Optional[IMemoryEngine] = None, + message_engine: Optional[IMessageEngine] = None, + ) -> None: self.compute_engine = compute_engine self.memory_engine = memory_engine self.message_engine = message_engine - + @property - def compute(self) -> IComputeEngine: + def compute(self) -> Optional[IComputeEngine]: return self.compute_engine @property - def memory(self) -> IMemoryEngine: + def memory(self) -> Optional[IMemoryEngine]: return self.memory_engine - + @property - def message(self) -> IMessageEngine: + def message(self) -> Optional[IMessageEngine]: return self.message_engine - - def terminate(self): - self.compute_engine.terminate() - self.memory_engine.terminate() - self.message_engine.terminate() + + def terminate(self) -> None: + if self.compute_engine is not None: + self.compute_engine.terminate() + if self.memory_engine is not None: + self.memory_engine.terminate() + if self.message_engine is not None: + self.message_engine.terminate() diff --git a/gamms/osm.py b/gamms/osm.py index 35c1637..0533182 100644 --- a/gamms/osm.py +++ b/gamms/osm.py @@ -3,11 +3,16 @@ except ImportError: raise ImportError('Please install osmnx to use this feature. pip install osmnx') +from geopandas import GeoDataFrame import networkx as nx -from shapely.geometry import LineString, Point +from shapely.geometry import LineString, Point, Polygon, MultiPolygon from enum import Enum +from typing import Any, Dict, Iterator, List, Optional, Tuple, Union, cast from copy import deepcopy as copy + +from .osm_constants import OSM_OBSTACLE_TAGS, HEIGHT_ESTIMATES_TYPES + class OSMType(Enum): WALK = 0 BIKE = 1 @@ -102,11 +107,12 @@ def graph_from_xml( resolution: float = 10.0, bidirectional: bool = True, retain_all: bool = False, - tolerance: int = 1e-9, + tolerance: float = 1e-9, ) -> nx.DiGraph: osmg = ox.graph.graph_from_xml(filepath, bidirectional=bidirectional, simplify=False, retain_all=retain_all) osmg = ox.project_graph(osmg) osmg = ox.consolidate_intersections(osmg, tolerance=tolerance, rebuild_graph=True, dead_ends=True) + osmg = cast(nx.MultiDiGraph, osmg) return process_osm_graph(osmg, resolution=resolution, bidirectional=bidirectional) def create_osm_graph( @@ -116,8 +122,8 @@ def create_osm_graph( simplify: bool = True, retain_all: bool = False, truncate_by_edge: bool = True, - custom_filter: str = None, - tolerance: int =10.0 + custom_filter: Optional[str] = None, + tolerance: float = 10.0 ) -> nx.DiGraph: resolution = float(resolution) osmg = ox.graph_from_place( @@ -135,5 +141,164 @@ def create_osm_graph( bidirectional = True else: bidirectional = False - + osmg = cast(nx.MultiDiGraph, osmg) return process_osm_graph(osmg, resolution=resolution, bidirectional=bidirectional) + +def extract_osm_polygon_faces( + gdf: GeoDataFrame, + height_estimates: Dict[Tuple[str, str], Tuple[float, int]] = HEIGHT_ESTIMATES_TYPES, + min_tolerance: float = 0.5, + relative_tolerance: float = 0.01, +) -> Iterator[Dict[str, Union[int, Tuple[float, float, float]]]]: + """Extract polygon records from a GeoDataFrame of OSM features. + + Each record is a dict with ``id``, ``coords``, ``height``, ``base``, + ``category``, and ``attributes``. Heights are resolved from common OSM + tags (``height``, ``building:levels``) and fall back to estimates based on + the feature's tags and the provided mapping. + + Args: + gdf: GeoDataFrame containing OSM features, typically obtained via + :func:`osmnx.features_from_place` or similar. + height_estimates: Mapping of (key, value) tag pairs to (height, type_code) + tuples used as fallbacks when explicit height data is missing. + min_tolerance: Minimum tolerance for the polygon's simplification. + relative_tolerance: Relative tolerance fraction for the polygon's simplification. + Fraction is based on polygon's length, so larger polygons get more aggressive simplification. + + Yields: + Dict with keys: + face_id: int, + tr: Tuple[float, float, float], + tl: Tuple[float, float, float], + br: Tuple[float, float, float], + bl: Tuple[float, float, float], + type: int, + """ + # Filter all non-polygon features + gdf = gdf[gdf.geometry.type.isin(["Polygon", "MultiPolygon"])] + # Add types in height_estimates as a new column for easier filtering + # If not applicable, set to NaN + gdf["type"] = None + gdf['height_estimate'] = None + for key, value in height_estimates.keys(): + try: + mask = (gdf[key] == value) + gdf.loc[mask, "type"] = height_estimates[(key, value)][1] + gdf.loc[mask, "height_estimate"] = height_estimates[(key, value)][0] + except KeyError: + continue + + # Filter to only features with a type + gdf = gdf[gdf["type"].notna()] + + if 'height' in gdf.columns: + # height -> meters + gdf["height"] = ( + gdf["height"] + .astype(str) + .str.lower() + .str.strip() + ) + + height_ft_mask = gdf["height"].str.contains("ft|feet", na=False) + + gdf["height"] = ( + gdf["height"] + .str.extract(r"([-+]?\d*\.?\d+)")[0] + .astype(float) + ) + + gdf.loc[height_ft_mask, "height"] *= 0.3048 + else: + gdf["height"] = float('nan') + + + if 'building:levels' in gdf.columns: + # building:levels -> numeric + gdf["building:levels"] = ( + gdf["building:levels"] + .astype(str) + .str.extract(r"([-+]?\d*\.?\d+)")[0] + .astype(float) + ) + else: + gdf["building:levels"] = float('nan') + + # priority: + # height > building:levels * 3 > existing height_estimate + gdf["height_estimate"] = ( + gdf["height"] + .fillna(gdf["building:levels"] * 3.0) + .fillna(gdf["height_estimate"]) + ) + next_id = 0 + for _, row in gdf.iterrows(): + geom = row.geometry + type_code = row["type"] + height = row["height_estimate"] + if isinstance(geom, MultiPolygon): + polygons = geom.geoms + else: + polygons = [geom] + for polygon in polygons: + # Simplify the polygon to reduce complexity, but ensure it remains valid and doesn't collapse + tolerance = max(min_tolerance, relative_tolerance * polygon.length) + simplified = polygon.simplify(tolerance, preserve_topology=True) + if not simplified.is_valid or simplified.is_empty: + continue + # Convert the simplified polygon into boundary linesegments and extract the corners + coords = tuple(simplified.exterior.coords) + coord_len = len(coords) + if coord_len < 3: + continue + for i in range(coord_len-1): + yield { + "face_id": next_id, + "tr": (coords[i][0], coords[i][1], height), + "tl": (coords[i+1][0], coords[i+1][1], height), + "br": (coords[i][0], coords[i][1], 0.0), + "bl": (coords[i+1][0], coords[i+1][1], 0.0), + "type": type_code, + } + next_id += 1 + +def obstacle_from_osm( + location: str, + tags: Dict[str, List[str]] = OSM_OBSTACLE_TAGS, + height_estimates: Dict[Tuple[str, str], Tuple[float, int]] = HEIGHT_ESTIMATES_TYPES, + min_tolerance: float = 0.5, + relative_tolerance: float = 0.01, +) -> Iterator[Dict[str, Union[int, Tuple[float, float, float]]]]: + gdf = ox.features_from_place( + location, + tags=tags, + ) + gdf = ox.projection.project_gdf(gdf) + return extract_osm_polygon_faces( + gdf, + height_estimates=height_estimates, + min_tolerance=min_tolerance, + relative_tolerance=relative_tolerance + ) + +def obstacle_from_xml( + filepath: str, + tags: Dict[str, List[str]] = OSM_OBSTACLE_TAGS, + height_estimates: Dict[Tuple[str, str], Tuple[float, int]] = HEIGHT_ESTIMATES_TYPES, + min_tolerance: float = 0.5, + relative_tolerance: float = 0.01, +) -> Iterator[Dict[str, Union[int, Tuple[float, float, float]]]]: + gdf = ox.features_from_xml( + filepath, + tags=tags, + ) + gdf = ox.projection.project_gdf(gdf) + return extract_osm_polygon_faces( + gdf, + height_estimates=height_estimates, + min_tolerance=min_tolerance, + relative_tolerance=relative_tolerance + ) + +__all__ = ["create_osm_graph", "graph_from_xml", "obstacle_from_osm", "obstacle_from_xml", "OSMType"] \ No newline at end of file diff --git a/gamms/osm_constants.py b/gamms/osm_constants.py new file mode 100644 index 0000000..69d03ef --- /dev/null +++ b/gamms/osm_constants.py @@ -0,0 +1,149 @@ +# OSMnx tag filters for features that should be treated as obstacles in the graph. +OSM_OBSTACLE_TAGS = { + "building": True, + "building:part": True, + + "landuse": [ + "industrial", + "commercial", + "retail", + "residential", + "construction", + "forest", + "military", + "railway", + ], + + "natural": [ + "wood", + "forest", + "scrub", + ], + + "leisure": [ + "sports_centre", + "stadium", + ], + + "amenity": [ + "school", + "university", + "hospital", + "parking", + ], + + "man_made": [ + "bridge", + "tower", + "water_tower", + "storage_tank", + "silo", + "chimney", + "communications_tower", + "mast", + ], + + "power": [ + "substation", + "generator", + "plant", + ], + + "aeroway": [ + "terminal", + "hangar", + ], +} + +# Used when explicit OSM height/building:levels data is missing +# These are rough estimates based on typical building types and land uses, and common vegetation heights. +# It also associates a type code for each category primarily for visualization purposes +HEIGHT_ESTIMATES_TYPES = { + + # Buildings + ("building", "house"): (8.0, 0), + ("building", "residential"): (10.0, 1), + ("building", "apartments"): (18.0, 2), + ("building", "commercial"): (15.0, 3), + ("building", "retail"): (12.0, 4), + ("building", "industrial"): (14.0, 5), + ("building", "warehouse"): (11.0, 6), + ("building", "school"): (12.0, 7), + ("building", "hospital"): (20.0, 8), + ("building", "university"): (18.0, 9), + ("building", "church"): (25.0, 10), + ("building", "garage"): (4.0, 11), + ("building", "hangar"): (16.0, 12), + ("building", "stadium"): (35.0, 13), + ("building", "yes"): (10.0, 14), + + # Forest / vegetation + ("natural", "wood"): (15.0, 15), + ("natural", "forest"): (18.0, 16), + ("landuse", "forest"): (18.0, 16), + + # Leisure / parks + ("leisure", "sports_centre"): (12.0, 17), + ("leisure", "stadium"): (35.0, 18), + + # Industrial/man-made + ("man_made", "tower"): (45.0, 19), + ("man_made", "water_tower"): (35.0, 20), + ("man_made", "storage_tank"): (18.0, 21), + ("man_made", "silo"): (30.0, 22), + ("man_made", "chimney"): (55.0, 23), + ("man_made", "communications_tower"): (50.0, 24), + ("man_made", "mast"): (40.0, 25), + ("man_made", "bridge"): (12.0, 26), + + # Power infrastructure + ("power", "substation"): (8.0, 27), + ("power", "plant"): (25.0, 28), + ("power", "generator"): (6.0, 29), + + # Aeroway + ("aeroway", "terminal"): (18.0, 30), + ("aeroway", "hangar"): (16.0, 31), + + # Landuse approximations + ("landuse", "industrial"): (14.0, 32), + ("landuse", "commercial"): (15.0, 33), + ("landuse", "retail"): (12.0, 34), + ("landuse", "residential"): (10.0, 35), + ("landuse", "military"): (12.0, 36), +} + +COLOR_TYPES = { + 0: "#a6cee3", # house + 1: "#1f78b4", # residential + 2: "#b2df8a", # apartments + 3: "#9fadf4", # commercial + 4: "#fb9a99", # retail + 5: "#e31a1c", # industrial + 6: "#fdbf6f", # warehouse + 7: "#ff7f00", # school + 8: "#cab2d6", # hospital + 9: "#6a3d9a", # university + 10: "#ffff99", # church + 11: "#b15928", # garage + 12: "#8dd3c7", # hangar + 13: "#ffffb3", # stadium + 14: "#bebada", # generic building + 15: "#fb8072", # wood + 16: "#33a02c", # forest + 17: "#fdb462", # sports_centre + 18: "#b3de69", # stadium (leisure) + 19: "#fccde5", # tower + 20: "#d9d9d9", # water_tower + 21: "#bc80bd", # storage_tank + 22: "#ccebc5", # silo + 23: "#ffed6f", # chimney + 24: "#8dd3c7", # communications_tower + 25: "#80b1d3", # mast + 26: "#fdb462", # bridge + 27: "#b3de69", # substation + 28: "#fccde5", # plant + 29: "#d9d9d9", # generator + 30: "#bc80bd", # terminal + 31: "#ccebc5", # hangar (aeroway) +} \ No newline at end of file diff --git a/gamms/typing/__init__.py b/gamms/typing/__init__.py index 85bbb5e..09d361b 100644 --- a/gamms/typing/__init__.py +++ b/gamms/typing/__init__.py @@ -1,12 +1,12 @@ from gamms.typing.compute_engine import IComputeEngine, ITask -from gamms.typing.memory_engine import IMemoryEngine +from gamms.typing.memory_engine import IMemoryEngine, IStore, IPathLike, StoreType from gamms.typing.message_engine import IMessageEngine from gamms.typing.internal_context import IInternalContext from gamms.typing.sensor_engine import ISensorEngine, ISensor, SensorType from gamms.typing.artist import IArtist, ArtistType from gamms.typing.visualization_engine import IVisualizationEngine, ColorType from gamms.typing.agent_engine import IAgentEngine, IAgent, IAerialAgent, AgentType -from gamms.typing.graph_engine import IGraphEngine, IGraph, OSMEdge, Node +from gamms.typing.graph_engine import IGraphEngine, IGraph, OSMEdge, Node, ObsFace from gamms.typing.recorder import IRecorder from gamms.typing.logger import ILogger from gamms.typing.context import IContext diff --git a/gamms/typing/artist.py b/gamms/typing/artist.py index b0f9893..dd6d4e0 100644 --- a/gamms/typing/artist.py +++ b/gamms/typing/artist.py @@ -3,9 +3,8 @@ from enum import Enum, auto class ArtistType(Enum): - GENERAL = auto() - AGENT = auto() - GRAPH = auto() + STATIC = auto() + DYNAMIC = auto() class IArtist(ABC): @@ -78,26 +77,6 @@ def get_drawer(self) -> Optional[Callable[["IContext", Dict[str, Any]], None]]: """ pass - @abstractmethod - def get_will_draw(self) -> bool: - """ - Get whether the artist will draw. - - Returns: - bool: True if the artist will draw, False otherwise. - """ - pass - - @abstractmethod - def set_will_draw(self, will_draw: bool) -> None: - """ - Set whether the artist will draw. - - Args: - will_draw (bool): The will_draw state to set. - """ - pass - @abstractmethod def get_artist_type(self) -> ArtistType: """ diff --git a/gamms/typing/graph_engine.py b/gamms/typing/graph_engine.py index 6922b26..72e2fab 100644 --- a/gamms/typing/graph_engine.py +++ b/gamms/typing/graph_engine.py @@ -1,5 +1,5 @@ from abc import ABC, abstractmethod -from typing import Any, Dict, Iterator, overload +from typing import Any, Dict, Iterator, Mapping, Tuple, Union, overload from shapely.geometry import LineString import networkx as nx @@ -48,6 +48,25 @@ class OSMEdge: length: float linestring: LineString +class ObsFace: + """ + Represents an obstacle face within a graph engine. + + Attributes: + id (int): The unique identifier for the obstacle face. + tr (Tuple[float, float, float]): Coordinates of the top-right corner of the face. + tl (Tuple[float, float, float]): Coordinates of the top-left corner of the face. + br (Tuple[float, float, float]): Coordinates of the bottom-right corner of the face. + bl (Tuple[float, float, float]): Coordinates of the bottom-left corner of the face. + type (int): An integer representing the type/category of the obstacle. + """ + id: int + tr: Tuple[float, float, float] + tl: Tuple[float, float, float] + br: Tuple[float, float, float] + bl: Tuple[float, float, float] + type: int + class IGraph(ABC): """ @@ -87,7 +106,7 @@ def add_edge(self, edge_data: Dict[str, Any]) -> None: - 'linestring' (List[Tuple[float, float]], optional): Geometry of the edge. Raises: - ValueError: If the edge_data is missing required fields, contains invalid data, or references non-existent nodes. + ValueError: If the edge_data is missing required fields, contains invalid data. KeyError: If an edge with the same ID already exists in the graph. KeyError: If source or target nodes do not exist in the graph. """ @@ -266,6 +285,87 @@ def graph(self) -> IGraph: """ pass + @abstractmethod + def add_obstacle_face( + self, + face_id: int, + tr: Tuple[float, float, float], + tl: Tuple[float, float, float], + br: Tuple[float, float, float], + bl: Tuple[float, float, float], + type: int + ) -> None: + """ + Add an obstacle face to the graph engine. + + Args: + face_id (int): Unique identifier for the obstacle face. + tr (Tuple[float, float, float]): Coordinates of the top-right corner of the face. + tl (Tuple[float, float, float]): Coordinates of the top-left corner of the face. + br (Tuple[float, float, float]): Coordinates of the bottom-right corner of the face. + bl (Tuple[float, float, float]): Coordinates of the bottom-left corner of the face. + type (int): An integer representing the type/category of the obstacle. + + Raises: + ValueError: If any of the provided coordinates are invalid or if the type is not recognized. + KeyError: If an obstacle face with the same ID already exists in the engine. + """ + pass + + @abstractmethod + def remove_obstacle_face(self, face_id: int) -> None: + """ + Remove an obstacle face from the graph engine. + + Args: + face_id (int): The unique identifier of the obstacle face to be removed. + Raises: + KeyError: If the obstacle face with the specified ID does not exist. + """ + pass + + @abstractmethod + def get_obstacle_face(self, face_id: int) -> ObsFace: + """ + Retrieve the attributes of a specific obstacle face. + + Args: + face_id (int): The unique identifier of the obstacle face to retrieve. + + Returns: + ObsFace: An instance of ObsFace containing the obstacle face's attributes. + + Raises: + KeyError: If the obstacle face with the specified ID does not exist. + """ + pass + + @abstractmethod + @overload + def get_obstacle_faces(self) -> Iterator[int]: + """ + Get the IDs of all obstacle faces in the graph engine. + + Returns: + Iterator[int]: An iterator that yields the IDs of all obstacle faces. + """ + pass + + @abstractmethod + @overload + def get_obstacle_faces(self, d: float, x: float, y: float) -> Iterator[int]: + """ + Get the IDs of obstacle faces within a certain distance from a point. + + Args: + d (float): The distance threshold. If d is non-negative, it returns obstacle faces within distance d from the point (x, y). + x (float): The x-coordinate of the reference point. + y (float): The y-coordinate of the reference point. + Returns: + Iterator[int]: An iterator that yields the IDs of obstacle faces within the specified distance. + """ + pass + @abstractmethod def attach_networkx_graph(self, G: nx.Graph) -> IGraph: """ diff --git a/gamms/typing/memory_engine.py b/gamms/typing/memory_engine.py index 15bb181..5ffe70f 100644 --- a/gamms/typing/memory_engine.py +++ b/gamms/typing/memory_engine.py @@ -1,150 +1,251 @@ from abc import ABC, abstractmethod -from typing import Any, List -from enum import Enum +from typing import Any, Dict, Iterator, List, Optional, Tuple, Type, Mapping +from enum import IntEnum + + +class StoreType(IntEnum): + """ + Enumeration of supported store backends. + + Attributes: + MEMORY: Pure in-memory store backed by Python dictionaries. + FILESYSTEM: Filesystem-backed store (kept for compatibility with the + previous artefact-style API). Not used by the structured + map/table backends. + DATABASE: SQLite-backed store with on-disk persistence. + """ + MEMORY = 0 + FILESYSTEM = 1 + DATABASE = 2 class IPathLike(ABC): """ Abstract base class representing a path-like object. - This interface defines the structure for objects that behave like filesystem paths. - It can be extended to support various path representations and operations. + PathLike objects are used to identify the on-disk (or remote) location of a + store. Implementations should normalise the path so that absolute paths + can be converted to a representation appropriate for the backend. """ - pass + + @abstractmethod + def as_str(self) -> str: + """ + Return the path as a string. + """ + pass + + @abstractmethod + def exists(self) -> bool: + """ + Return True if the path exists on the underlying medium. + """ + pass class IStore(ABC): """ - Abstract base class representing a generic storage mechanism. + Abstract base class representing a structured storage instance. - The store provides methods to save, load, and delete objects, facilitating - persistent storage and retrieval of data. + Primarily this is so that there is an abstraction over storage type + Based on individual implementations, there can be a variety in implementation + + The objective is that there are multiple places where basic storage operations are needed + but want it to be agnostic to the underlying storage implementation. The base API is not + supposed to be optimal way to access it. """ @abstractmethod - def save(self, obj: Any) -> None: + def name(self) -> str: + """The unique name of this store.""" + pass + + @abstractmethod + def path(self) -> Optional[IPathLike]: """ - Save an object to the storage. + The path associated with this store, if applicable. - This method persists the provided object to the underlying storage medium. + Returns: + An IPathLike object if the store has an associated path, or None if the store is purely in-memory. + """ + pass + + @property + @abstractmethod + def type(self) -> StoreType: + """The backend type of this store.""" + pass + + @abstractmethod + def create_map(self, map_name: str, schema: Dict[str, Type], primary_key: str) -> None: + """ + Create a new key/value map within the store. Args: - obj (Any): The object to be saved. It can be of any type that the store supports. + map_name: Unique name of the map within this store. + schema: Defines the schema for the map. + primary_key: The key to use as the primary key for the map. It needs to be unique within the map. Raises: - IOError: If an error occurs during the save operation. - ValueError: If the object is invalid or cannot be serialized. + ValueError: If a map with the given name already exists. + TypeError: If there is unsupported strutures in the schema. + IndexError: If the primary key is not found in the schema or is not indexable """ pass @abstractmethod - def load(self) -> Any: + def delete_map(self, map_name: str) -> None: """ - Load and retrieve an object from the storage. - - This method fetches the stored object from the underlying storage medium. + Delete a map from the store. - Returns: - Any: The retrieved object. The type depends on what was originally saved. + Args: + map_name: Name of the map to delete. Raises: - IOError: If an error occurs during the load operation. - FileNotFoundError: If there is no object to load. - ValueError: If the stored data is corrupted or cannot be deserialized. + KeyError: If the map does not exist. """ pass @abstractmethod - def delete(self) -> None: + def list_maps(self) -> List[str]: + """Return the names of all maps currently in the store.""" + pass + + @abstractmethod + def insert_data(self, map_name: str, struct: Dict[str, Any]) -> None: """ - Delete the stored object from the storage. + Insert a key/value pair into a map. - This method removes the persisted object from the underlying storage medium. + Args: + map_name: Name of the target map. + struct: A dictionary containing the key-value pairs to insert. Raises: - IOError: If an error occurs during the delete operation. - FileNotFoundError: If there is no object to delete. + IndexError: If the map does not exist. + KeyError: If there is an issue with the primary key. + ValueError: If there is an issue with struct insertion """ pass + @abstractmethod + def get_data(self, map_name: str, key: Any) -> Mapping[str, Any]: + """ + Retrieve the value associated with the key -class IMemoryEngine(ABC): - """ - Abstract base class representing a memory engine for managing stores. + Args: + map_name: Name of the target map. + key: The key for which to retrieve the value. - The memory engine is responsible for creating, listing, loading, and terminating - storage stores. It acts as a manager that oversees various storage instances. - """ + Raises: + IndexError: If the map does not exist. + KeyError: If the key is not found in the map. + """ + pass @abstractmethod - def create_store(self, store_type: Enum, name: str, path: IPathLike) -> IStore: + def update_data(self, map_name: str, struct: Dict[str, Any]) -> None: """ - Create a new store within the memory engine. - - This method initializes a new storage instance based on the specified type, - assigns it a name, and associates it with a path-like object. + Update an entry in the map Args: - store_type (Enum): An enumeration specifying the type of store to create. - This could represent different storage backends or configurations. - name (str): The unique name identifier for the store. - path (IPathLike): A path-like object specifying where the store's data - should be located or managed. - - Returns: - IStore: The newly created store instance. + map_name: Name of the target map. + struct: A dictionary containing the key-value pairs to update. Raises: - ValueError: If the provided store_type is unsupported or invalid. - FileExistsError: If a store with the given name already exists. - IOError: If there is an issue creating the store at the specified path. + IndexError: If the map does not exist. + KeyError: If there is an issue with the primary key. + ValueError: If there is an issue with struct update """ pass @abstractmethod - def list_stores(self) -> List[str]: + def delete_data(self, map_name: str, key: Any) -> None: """ - List all store names managed by the memory engine. - - This method retrieves the names of all existing stores within the engine. + Delete a key from a map - Returns: - List[str]: A list of store names currently managed by the engine. + Args: + map_name: Name of the target map. + key: The key to delete. Raises: - RuntimeError: If the memory engine is not properly initialized. + IndexError: If the map does not exist. + KeyError: If the key is not found in the map. """ pass @abstractmethod - def load_store(self, name: str) -> IStore: + def query_keys(self, map_name: str) -> Iterator[Any]: + """ + Query all keys in a map. + + Args: + map_name: Name of the target map. + + Raises: + IndexError: If the map does not exist. """ - Load an existing store by its name. + pass + + @abstractmethod + def close(self) -> None: + """Release any resources held by the store.""" + pass + + +class IMemoryEngine(ABC): + """ + Abstract base class for the memory engine. - This method retrieves a store instance based on its unique name identifier. + The memory engine is a low level abstraction over different storage + backends. It owns a collection of named stores; each store is itself a + keyed collection of typed maps. + """ + + @abstractmethod + def create_store( + self, + store_type: StoreType, + name: str, + path: Optional[IPathLike] = None, + ) -> IStore: + """ + Create a new store and register it with the engine. Args: - name (str): The unique name identifier of the store to load. + store_type: Backend type for the new store. + name: Unique store name. + path: Optional path for backends that require persistence. Returns: - IStore: The loaded store instance. + IStore: The newly created store instance. Raises: - KeyError: If no store with the specified name exists. - IOError: If there is an issue accessing the store's data. + ValueError: If a store with the same name already exists, or if + the requested ``store_type`` is unsupported. """ pass @abstractmethod - def terminate(self) -> None: + def get_store(self, name: str) -> IStore: """ - Terminate the memory engine and perform necessary cleanup operations. + Retrieve an existing store. - This method ensures that all stores are properly closed and that any - allocated resources are released. It prepares the engine for shutdown. + Args: + name: Unique name of the store. Raises: - RuntimeError: If the engine fails to terminate gracefully. - IOError: If there are issues during the cleanup process. + KeyError: If no store with the specified name exists. + """ + pass + + @abstractmethod + def list_stores(self) -> Iterator[str]: """ + Return the names of all stores. + """ + pass + + @abstractmethod + def terminate(self) -> None: + """Tear down the memory engine and release all store resources.""" pass diff --git a/gamms/typing/sensor_engine.py b/gamms/typing/sensor_engine.py index d96da39..c8cf70b 100644 --- a/gamms/typing/sensor_engine.py +++ b/gamms/typing/sensor_engine.py @@ -40,6 +40,10 @@ class SensorType(Enum): AGENT_ARC = 7 AERIAL = 8 AERIAL_AGENT = 9 + OCCLUDED_MAP = 10 + OCCLUDED_AGENT = 11 + OCCLUDED_AERIAL = 12 + OCCLUDED_AERIAL_AGENT = 13 class ISensor(ABC): diff --git a/gamms/typing/visualization_engine.py b/gamms/typing/visualization_engine.py index eccae6e..21e3592 100644 --- a/gamms/typing/visualization_engine.py +++ b/gamms/typing/visualization_engine.py @@ -40,6 +40,25 @@ def set_graph_visual(self, **kwargs: Dict[str, Any]) -> IArtist: """ pass + @abstractmethod + def set_obstacle_visual(self, **kwargs: Dict[str, Any]) -> IArtist: + """ + Configure the visual representation of obstacles in the graph. + This method sets up visual parameters for obstacles, allowing customization + of how they are displayed within the visualization. + + Args: + **kwargs: Arbitrary keyword arguments representing visual settings for obstacles. + Possible keys include: + - `color_map` (Dict[int, ColorType]): A mapping of obstacle types to their corresponding colors. + - `boundary_thickness` (float): The thickness of the obstacle boundaries in the visualization. + - Additional visual parameters specific to obstacles as needed. + Raises: + ValueError: If any of the provided visual settings are invalid. + TypeError: If the types of the provided settings do not match expected types. + """ + pass + @abstractmethod def set_agent_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: """ @@ -55,6 +74,7 @@ def set_agent_visual(self, name: str, **kwargs: Dict[str, Any]) -> IArtist: - `color` (str): The color to represent the agent. - `shape` (str): The shape to use for the agent's representation. - `size` (float): The size of the agent in the visualization. + - `image_path` (str): The file path to an image representing the agent. """ pass @@ -171,7 +191,7 @@ def terminate(self) -> None: pass @abstractmethod - def render_circle(self, x: float, y: float, radius: float, color: Tuple[Union[int, float], Union[int, float], Union[int, float]], width: int, perform_culling_test: bool): + def render_circle(self, x: float, y: float, radius: float, color: ColorType, width: int, perform_culling_test: bool): """ Render a circle shape at the specified position with the given radius and color. @@ -186,7 +206,7 @@ def render_circle(self, x: float, y: float, radius: float, color: Tuple[Union[in pass @abstractmethod - def render_rectangle(self, x: float, y: float, width: float, height: float, color: Tuple[Union[int, float], Union[int, float], Union[int, float]], perform_culling_test: bool): + def render_rectangle(self, x: float, y: float, width: float, height: float, color: ColorType, perform_culling_test: bool): """ Render a rectangle shape at the specified position with the given dimensions and color. @@ -201,7 +221,7 @@ def render_rectangle(self, x: float, y: float, width: float, height: float, colo pass @abstractmethod - def render_line(self, start_x: float, start_y: float, end_x: float, end_y: float, color: Tuple[Union[int, float], Union[int, float], Union[int, float]], width: int, is_aa: bool, perform_culling_test: bool): + def render_line(self, start_x: float, start_y: float, end_x: float, end_y: float, color: ColorType, width: int, is_aa: bool, perform_culling_test: bool): """ Render a line segment between two points with the specified color and width. @@ -218,7 +238,7 @@ def render_line(self, start_x: float, start_y: float, end_x: float, end_y: float pass @abstractmethod - def render_linestring(self, points: List[Tuple[float, float]], color: Tuple[Union[int, float], Union[int, float], Union[int, float]], width: int, closed: bool, is_aa: bool, perform_culling_test: bool): + def render_linestring(self, points: List[Tuple[float, float]], color: ColorType, width: int, closed: bool, is_aa: bool, perform_culling_test: bool): """ Render a series of connected line segments between multiple points. @@ -233,7 +253,7 @@ def render_linestring(self, points: List[Tuple[float, float]], color: Tuple[Unio pass @abstractmethod - def render_polygon(self, points: List[Tuple[float, float]], color: Tuple[Union[int, float], Union[int, float], Union[int, float]], width: int, + def render_polygon(self, points: List[Tuple[float, float]], color: ColorType, width: int, perform_culling_test: bool): """ Render a polygon shape or outline defined by a list of vertices with the specified color and width. @@ -247,7 +267,22 @@ def render_polygon(self, points: List[Tuple[float, float]], color: Tuple[Union[i pass @abstractmethod - def render_text(self, text: str, x: float, y: float, color: Tuple[Union[int, float], Union[int, float], Union[int, float]], perform_culling_test: bool, font_size: Optional[int]): + def render_image(self, x: float, y: float, image: Any, size: float, angle: float, perform_culling_test: bool): + """ + Render an image centered on a world-space position. + + Args: + x (float): The x-coordinate of the image center. + y (float): The y-coordinate of the image center. + image (Any): The loaded image surface. + size (float): The image size in world units, treated like an agent radius. + angle (float): Rotation angle in radians. + perform_culling_test (bool): Whether to cull the image before drawing. + """ + pass + + @abstractmethod + def render_text(self, text: str, x: float, y: float, color: ColorType, perform_culling_test: bool, font_size: Optional[int]): """ Render text at the specified position with the given content and color. @@ -270,3 +305,16 @@ def render_layer(self, layer_id: int) -> None: layer_id (int): The layer number to render. """ pass + + @abstractmethod + def get_viewport(self) -> Optional[Tuple[float, float, float, float, float]]: + """ + Return the current viewport as (left, right, top, bottom, scale) in world + coordinates, where scale is the pixels-per-world-unit factor at the + current camera zoom. + + Returns: + Optional[Tuple[float, float, float, float, float]]: (left, right, top, + bottom, scale) in world coordinates, or None if there is no valid viewport. + """ + pass diff --git a/pyproject.toml b/pyproject.toml index 1105f21..fcaf1b6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,12 +6,12 @@ build-backend = "setuptools.build_meta" [project] name = "gamms" -version = "0.2.7" +version = "1.0.0" authors = [ {name = "Rohan Patil", email = "rpatil@ucsd.edu"}, {name = "Jai Malegaonkar", email = "jmalegaonkar@ucsd.edu"}, - {name = "Andre Dion"}, - {name = "Xiao Jiang"}, + {name = "Mehul Sinha"}, + {name = "Jinmin Lee"}, ] description = "GAMMS (Graph based Adversarial Multiagent Modelling Simulator) is a Python library designed for simulating large scale multi-agent scenarios on environments represented as graphs" readme = "README.md" @@ -23,6 +23,8 @@ classifiers = [ "Programming Language :: Python :: 3.9", "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: 3.13", "Intended Audience :: Developers", "Intended Audience :: Science/Research", "Operating System :: OS Independent" @@ -33,7 +35,7 @@ dependencies = [ "networkx", "cbor2<=5.7.1", "aenum", - "osmnx" + "osmnx !=2.0.5,!=2.0.6,!=2.0.7,!=2.0.8,!=2.0.9", ] [tool.setuptools.packages.find] diff --git a/tests/agent_test.py b/tests/agent_test.py index 3074b3a..c59132d 100644 --- a/tests/agent_test.py +++ b/tests/agent_test.py @@ -142,8 +142,9 @@ def test_engine(self): def suite(): suite = unittest.TestSuite() - suite.addTest(unittest.makeSuite(AgentTest)) - suite.addTest(unittest.makeSuite(AgentEngineTest)) + loader = unittest.TestLoader() + suite.addTest(loader.loadTestsFromTestCase(AgentTest)) + suite.addTest(loader.loadTestsFromTestCase(AgentEngineTest)) return suite if __name__ == '__main__': diff --git a/tests/graph_test.py b/tests/graph_test.py index d144147..b023459 100644 --- a/tests/graph_test.py +++ b/tests/graph_test.py @@ -178,6 +178,59 @@ def test_get_neighbors(self): self.assertIn(1, neighbors) self.assertIn(3, neighbors) + def test_obstacle_face(self): + self.ctx.graph.add_obstacle_face( + face_id=1, + tr=(1.0, 1.0, 0.0), + tl=(0.0, 1.0, 0.0), + br=(1.0, 0.0, 0.0), + bl=(0.0, 0.0, 0.0), + type=0 + ) + + with self.assertRaises(KeyError): + self.ctx.graph.add_obstacle_face( + face_id=1, + tr=(1.0, 1.0, 0.0), + tl=(0.0, 1.0, 0.0), + br=(1.0, 0.0, 0.0), + bl=(0.0, 0.0, 0.0), + type=0 + ) + + with self.assertRaises(ValueError): + self.ctx.graph.add_obstacle_face( + face_id=2, + tr=(1.0, 1.0), + tl=(0.0, 1.0, 0.0), + br=(1.0, 0.0, 0.0), + bl=(0.0, 0.0, 0.0), + type=-1 + ) + + with self.assertRaises(KeyError): + self.ctx.graph.remove_obstacle_face(2) + + with self.assertRaises(KeyError): + self.ctx.graph.get_obstacle_face(2) + + self.ctx.graph.get_obstacle_face(1) + + face_ids = list(self.ctx.graph.get_obstacle_faces()) + self.assertEqual(len(face_ids), 1) + self.assertIn(1, face_ids) + + face_ids = list(self.ctx.graph.get_obstacle_faces(d=10, x=0, y=0)) + self.assertEqual(len(face_ids), 1) + self.assertIn(1, face_ids) + + face_ids = list(self.ctx.graph.get_obstacle_faces(d=2.0, x=100, y=100)) + self.assertEqual(len(face_ids), 0) + + self.ctx.graph.remove_obstacle_face(1) + with self.assertRaises(KeyError): + self.ctx.graph.get_obstacle_face(1) + def test_attach_network(self): with self.assertRaises(ValueError): self.ctx.graph.attach_networkx_graph(None) diff --git a/tests/memory_test.py b/tests/memory_test.py new file mode 100644 index 0000000..eb0bb7f --- /dev/null +++ b/tests/memory_test.py @@ -0,0 +1,142 @@ +import unittest + +import gamms +from gamms.MemoryEngine.store import PathLike + + +class StoreTestBase(unittest.TestCase): + def test_create_delete_map(self): + self.store.create_map('m', {'id': int, 'name': str}, 'id') + self.assertIn('m', self.store.list_maps()) + + with self.assertRaises(ValueError): + self.store.create_map('m', {'id': int, 'name': str}, 'id') + + self.store.delete_map('m') + + with self.assertRaises(IndexError): + self.store.create_map('m', {'id': int, 'name': tuple}, 'name') + + with self.assertRaises(IndexError): + self.store.create_map('m', {'id': int, 'name': tuple}, 'tag') + + with self.assertRaises(KeyError): + self.store.delete_map('m') + + def test_insert_get_update_data(self): + self.store.create_map('m', {'id': int, 'name': str}, 'id') + self.store.insert_data('m', {'id': 1, 'name': 'foo'}) + data = self.store.get_data('m', 1) + self.assertEqual(data['id'], 1) + self.assertEqual(data['name'], 'foo') + + with self.assertRaises(IndexError): + self.store.insert_data('q', {'id': 1, 'name': 'bar'}) + + with self.assertRaises(KeyError): + self.store.insert_data('m', {'id': 1, 'name': 'bar'}) + + with self.assertRaises(ValueError): + self.store.insert_data('m', {'id': 2, 'tag': 'baz'}) + + with self.assertRaises(IndexError): + self.store.get_data('q', 1) + + with self.assertRaises(KeyError): + self.store.get_data('m', 999) + + with self.assertRaises(IndexError): + self.store.update_data('q', {'id': 1, 'name': 'bar'}) + + with self.assertRaises(KeyError): + self.store.update_data('m', {'id': 3, 'name': 'baz'}) + + self.store.update_data('m', {'id': 1, 'name': 'qux'}) + data = self.store.get_data('m', 1) + self.assertEqual(data['id'], 1) + self.assertEqual(data['name'], 'qux') + + def test_delete_data(self): + self.store.create_map('m', {'id': int, 'name': str}, 'id') + self.store.insert_data('m', {'id': 1, 'name': 'foo'}) + self.store.delete_data('m', 1) + + with self.assertRaises(IndexError): + self.store.delete_data('q', 1) + + with self.assertRaises(KeyError): + self.store.delete_data('m', 1) + + + def test_list_maps(self): + self.store.create_map('m1', {'id': int}, 'id') + self.store.create_map('m2', {'id': int}, 'id') + maps = self.store.list_maps() + self.assertIn('m1', maps) + self.assertIn('m2', maps) + + def test_query_keys(self): + self.store.create_map('m', {'id': int, 'name': str}, 'id') + self.store.insert_data('m', {'id': 1, 'name': 'foo'}) + self.store.insert_data('m', {'id': 2, 'name': 'bar'}) + keys = list(self.store.query_keys('m')) + self.assertIn(1, keys) + self.assertIn(2, keys) + + with self.assertRaises(IndexError): + list(self.store.query_keys('q')) + + def tearDown(self) -> None: + return self.ctx.terminate() + +class MemoryStoreTest(StoreTestBase): + def setUp(self): + self.ctx = gamms.create_context(logger_config={'level': 'ERROR'}) + self.store = self.ctx.ictx.memory.create_store(gamms.typing.StoreType.MEMORY, 'test_store') + + +class SqliteStoreTest(StoreTestBase): + def setUp(self): + self.ctx = gamms.create_context(logger_config={'level': 'ERROR'}) + self.store = self.ctx.ictx.memory.create_store(gamms.typing.StoreType.DATABASE, 'test_store', path=PathLike(':memory:')) + +class MemoryEngineTestSuite(unittest.TestCase): + def setUp(self) -> None: + self.ctx = gamms.create_context(logger_config={'level': 'ERROR'}) + + def test_create_get_list_store(self): + store = self.ctx.ictx.memory.create_store(gamms.typing.StoreType.MEMORY, 'mem_store') + self.assertIsInstance(store, gamms.typing.IStore) + + with self.assertRaises(ValueError): + self.ctx.ictx.memory.create_store(gamms.typing.StoreType.MEMORY, 'mem_store') + + store2 = self.ctx.ictx.memory.create_store(gamms.typing.StoreType.DATABASE, 'sqlite_store', path=PathLike(':memory:')) + self.assertIsInstance(store2, gamms.typing.IStore) + + with self.assertRaises(ValueError): + self.ctx.ictx.memory.create_store(gamms.typing.StoreType.DATABASE, 'sqlite_store', path=PathLike(':memory:')) + + stores = list(self.ctx.ictx.memory.list_stores()) + self.assertIn('mem_store', stores) + self.assertIn('sqlite_store', stores) + + with self.assertRaises(KeyError): + self.ctx.ictx.memory.get_store('nonexistent_store') + + self.ctx.ictx.memory.get_store('mem_store') + self.ctx.ictx.memory.get_store('sqlite_store') + +def suite(): + s = unittest.TestSuite() + for cls in ( + MemoryEngineTestSuite, + MemoryStoreTest, + SqliteStoreTest, + ): + s.addTests(unittest.defaultTestLoader.loadTestsFromTestCase(cls)) + return s + + +if __name__ == '__main__': + unittest.TextTestRunner().run(suite()) diff --git a/tests/occlusion_test.py b/tests/occlusion_test.py new file mode 100644 index 0000000..a84dca2 --- /dev/null +++ b/tests/occlusion_test.py @@ -0,0 +1,666 @@ +""" +Occlusion sensor tests. + +""" + +import math +import unittest + +import numpy as np + +import gamms +import gamms.typing +import gamms.typing.agent_engine +from gamms.SensorEngine.sensors_occluded import ( + _quad_blocks, + _quad_blocks_batch, + _segment_triangle, +) + + +# --------------------------------------------------------------------------- +# Grid / building helpers shared by all test classes +# --------------------------------------------------------------------------- + +_GRID_N = 5 +_GRID_SPACING = 10.0 # metres between adjacent nodes +_WALL_HEIGHT = 8.0 # metres — tall enough to block eye-level rays + +_face_id_counter = 0 # module-level counter so IDs never collide across tests + + +def _next_face_ids(n: int): + global _face_id_counter + start = _face_id_counter + _face_id_counter += n + return range(start, start + n) + + +def _box_faces(x0: float, y0: float, x1: float, y1: float, height: float): + """4 ObsFace dicts for a closed rectangular building footprint.""" + corners = [(x0, y0), (x1, y0), (x1, y1), (x0, y1)] + ids = _next_face_ids(4) + faces = [] + for i, fid in enumerate(ids): + p1, p2 = corners[i], corners[(i + 1) % 4] + faces.append({ + 'id': fid, + 'tl': (p1[0], p1[1], height), + 'tr': (p2[0], p2[1], height), + 'br': (p2[0], p2[1], 0.0), + 'bl': (p1[0], p1[1], 0.0), + }) + return faces + + +# --------------------------------------------------------------------------- +# Geometry stubs (for unit tests that don't need the full context) +# --------------------------------------------------------------------------- + +class _Face: + __slots__ = ('tl', 'tr', 'br', 'bl') + + def __init__(self, tl, tr, br, bl): + self.tl = tl; self.tr = tr; self.br = br; self.bl = bl + + +# 1 m × 2 m wall at x=5, y ∈ [-0.5, 0.5], z ∈ [0, 2] +_UNIT_WALL = _Face( + tl=(5.0, -0.5, 2.0), tr=(5.0, 0.5, 2.0), + br=(5.0, 0.5, 0.0), bl=(5.0, -0.5, 0.0), +) + + +# --------------------------------------------------------------------------- +# Base class: 5 × 5 grid context +# --------------------------------------------------------------------------- + +class GridTest(unittest.TestCase): + """Sets up a 5×5 node grid and tears it down after each test.""" + + def setUp(self): + self.ctx = gamms.create_context( + vis_engine=gamms.visual.Engine.NO_VIS, + logger_config={'level': 'CRITICAL'}, + graph_engine=gamms.graph.Engine.MEMORY, + ) + self._build_grid() + + def tearDown(self): + self.ctx.terminate() + + # ---- grid construction ------------------------------------------------ + + def _build_grid(self): + g = gamms.create_context # just to satisfy linter, we use self.ctx below + g = self.ctx.graph.graph + N, S = _GRID_N, _GRID_SPACING + for row in range(N): + for col in range(N): + g.add_node({'id': self.nid(row, col), + 'x': col * S, 'y': row * S}) + eid = 0 + for row in range(N): + for col in range(N): + src = self.nid(row, col) + if col + 1 < N: + g.add_edge({'id': eid, 'source': src, + 'target': self.nid(row, col + 1), + 'length': S}) + eid += 1 + if row + 1 < N: + g.add_edge({'id': eid, 'source': src, + 'target': self.nid(row + 1, col), + 'length': S}) + eid += 1 + + def nid(self, row: int, col: int) -> int: + return row * _GRID_N + col + + def pos(self, row: int, col: int): + return col * _GRID_SPACING, row * _GRID_SPACING + + # ---- building helpers ------------------------------------------------- + + def add_building(self, x0, y0, x1, y1, height=_WALL_HEIGHT): + for f in _box_faces(x0, y0, x1, y1, height): + self.ctx.graph.add_obstacle_face( + f['id'], tl=f['tl'], tr=f['tr'], br=f['br'], bl=f['bl'], type=0, + ) + + def add_building_between(self, row0, col0, row1, col1, + thickness=2.0, height=_WALL_HEIGHT): + """Place a building slab on the midpoint of the edge (row0,col0)→(row1,col1).""" + x0, y0 = self.pos(row0, col0) + x1, y1 = self.pos(row1, col1) + mx, my = (x0 + x1) / 2, (y0 + y1) / 2 + if row0 == row1: # horizontal edge — wall perpendicular to x-axis + self.add_building(mx - 1, my - thickness, mx + 1, my + thickness, height) + else: # vertical edge — wall perpendicular to y-axis + self.add_building(mx - thickness, my - 1, mx + thickness, my + 1, height) + + # ---- sensor helpers --------------------------------------------------- + + def make_sensor(self, label, sensor_type, **kwargs): + return self.ctx.sensor.create_sensor(label, sensor_type, **kwargs) + + def occluded_map(self, label='occ', **kwargs): + return self.make_sensor(label, gamms.typing.SensorType.OCCLUDED_MAP, **kwargs) + + def occluded_agent(self, label='occ_agent', **kwargs): + return self.make_sensor(label, gamms.typing.SensorType.OCCLUDED_AGENT, **kwargs) + + def occluded_aerial(self, label='occ_aerial', **kwargs): + return self.make_sensor(label, gamms.typing.SensorType.OCCLUDED_AERIAL, **kwargs) + + def occluded_aerial_agent(self, label='occ_aerial_agent', **kwargs): + return self.make_sensor(label, gamms.typing.SensorType.OCCLUDED_AERIAL_AGENT, **kwargs) + + +# --------------------------------------------------------------------------- +# Scalar Möller-Trumbore unit tests +# --------------------------------------------------------------------------- + +class SegmentTriangleTest(unittest.TestCase): + """Triangle at x=5: v0=(5,-1,0), v1=(5,1,0), v2=(5,0,2).""" + + V0 = (5.0, -1.0, 0.0) + V1 = (5.0, 1.0, 0.0) + V2 = (5.0, 0.0, 2.0) + + def hit(self, a, b): + return _segment_triangle(a, b, self.V0, self.V1, self.V2) + + def test_centre_hit(self): + self.assertTrue(self.hit((0, 0, 0.67), (10, 0, 0.67))) + + def test_near_apex(self): + self.assertTrue(self.hit((0, 0, 1.9), (10, 0, 1.9))) + + def test_near_base_left(self): + self.assertTrue(self.hit((0, -0.9, 0.05), (10, -0.9, 0.05))) + + def test_miss_above_apex(self): + self.assertFalse(self.hit((0, 0, 2.1), (10, 0, 2.1))) + + def test_miss_below_base(self): + self.assertFalse(self.hit((0, 0, -0.1), (10, 0, -0.1))) + + def test_miss_left_of_triangle(self): + self.assertFalse(self.hit((0, -1.1, 0.5), (10, -1.1, 0.5))) + + def test_miss_right_of_triangle(self): + self.assertFalse(self.hit((0, 1.1, 0.5), (10, 1.1, 0.5))) + + def test_segment_stops_before_plane(self): + self.assertFalse(self.hit((0, 0, 0.67), (4.99, 0, 0.67))) + + def test_segment_endpoint_on_triangle(self): + self.assertTrue(self.hit((0, 0, 0.67), (5, 0, 0.67))) + + def test_segment_starts_past_triangle(self): + self.assertFalse(self.hit((6, 0, 0.67), (10, 0, 0.67))) + + def test_parallel_to_plane(self): + self.assertFalse(self.hit((0, 0, 1), (0, 10, 1))) + + def test_zero_length_segment(self): + self.assertFalse(self.hit((5, 0, 0.67), (5, 0, 0.67))) + + def test_ray_in_triangle_plane(self): + self.assertFalse(self.hit((5, -0.5, 0.5), (5, 0.5, 0.5))) + + def test_reversed_direction_still_hits(self): + self.assertTrue(self.hit((10, 0, 0.67), (0, 0, 0.67))) + + def test_origin_on_triangle(self): + self.assertTrue(self.hit((5, 0, 0.67), (10, 0, 0.67))) + + +# --------------------------------------------------------------------------- +# Scalar quad blocks unit tests +# --------------------------------------------------------------------------- + +class QuadBlocksTest(unittest.TestCase): + + def test_ray_hits_wall(self): + self.assertTrue(_quad_blocks((0, 0, 1), (10, 0, 1), _UNIT_WALL)) + + def test_angled_ray_hits(self): + self.assertTrue(_quad_blocks((0, -0.4, 0.5), (10, 0.4, 1.5), _UNIT_WALL)) + + def test_hit_lower_triangle(self): + self.assertTrue(_quad_blocks((0, -0.4, 0.1), (10, -0.4, 0.1), _UNIT_WALL)) + + def test_hit_upper_triangle(self): + self.assertTrue(_quad_blocks((0, 0.4, 1.8), (10, 0.4, 1.8), _UNIT_WALL)) + + def test_miss_wide_left(self): + self.assertFalse(_quad_blocks((0, -2, 1), (10, -2, 1), _UNIT_WALL)) + + def test_miss_wide_right(self): + self.assertFalse(_quad_blocks((0, 2, 1), (10, 2, 1), _UNIT_WALL)) + + def test_miss_above_wall(self): + self.assertFalse(_quad_blocks((0, 0, 2.5), (10, 0, 2.5), _UNIT_WALL)) + + def test_miss_below_wall(self): + self.assertFalse(_quad_blocks((0, 0, -0.5), (10, 0, -0.5), _UNIT_WALL)) + + def test_segment_stops_before_wall(self): + self.assertFalse(_quad_blocks((0, 0, 1), (4.9, 0, 1), _UNIT_WALL)) + + def test_both_endpoints_behind_wall(self): + self.assertFalse(_quad_blocks((6, 0, 1), (9, 0, 1), _UNIT_WALL)) + + def test_observer_behind_wall(self): + self.assertFalse(_quad_blocks((7, 0, 1), (12, 0, 1), _UNIT_WALL)) + + def test_diagonal_wall_hit(self): + diag = _Face( + tl=(3.0, 3.0, 4.0), tr=(7.0, 7.0, 4.0), + br=(7.0, 7.0, 0.0), bl=(3.0, 3.0, 0.0), + ) + self.assertTrue(_quad_blocks((0, 5, 2), (10, 5, 2), diag)) + + def test_diagonal_wall_parallel_miss(self): + diag = _Face( + tl=(3.0, 3.0, 4.0), tr=(7.0, 7.0, 4.0), + br=(7.0, 7.0, 0.0), bl=(3.0, 3.0, 0.0), + ) + self.assertFalse(_quad_blocks((0, 8, 2), (10, 8, 2), diag)) + + +# --------------------------------------------------------------------------- +# Vectorised batch unit tests +# --------------------------------------------------------------------------- + +class QuadBlocksBatchTest(unittest.TestCase): + + OBS = np.array([0.0, 0.0, 1.0]) + + def test_empty_returns_empty_bool_array(self): + result = _quad_blocks_batch(self.OBS, np.zeros((0, 3)), _UNIT_WALL) + self.assertEqual(len(result), 0) + self.assertEqual(result.dtype, bool) + + def test_single_blocked(self): + self.assertTrue(_quad_blocks_batch(self.OBS, np.array([[10.0, 0.0, 1.0]]), _UNIT_WALL)[0]) + + def test_single_clear(self): + self.assertFalse(_quad_blocks_batch(self.OBS, np.array([[10.0, 5.0, 1.0]]), _UNIT_WALL)[0]) + + def test_all_blocked(self): + targets = np.array([[10.0, 0.0, 0.5], [12.0, 0.0, 1.0], [15.0, 0.2, 1.5]]) + self.assertTrue(_quad_blocks_batch(self.OBS, targets, _UNIT_WALL).all()) + + def test_none_blocked(self): + targets = np.array([ + [10.0, 5.0, 1.0], # beside + [10.0, -5.0, 1.0], # beside + [ 3.0, 0.0, 1.0], # in front of wall + [10.0, 0.0, 5.0], # above (z=5 clears top at z=2) + ]) + self.assertFalse(_quad_blocks_batch(self.OBS, targets, _UNIT_WALL).any()) + + def test_mixed(self): + targets = np.array([ + [10.0, 0.0, 1.0], # blocked + [10.0, 5.0, 1.0], # clear — beside + [10.0, 0.0, 5.0], # clear — above + [10.0, -0.4, 0.2], # blocked — lower triangle + [ 3.0, 0.0, 1.0], # clear — in front + ]) + self.assertEqual(list(_quad_blocks_batch(self.OBS, targets, _UNIT_WALL)), + [True, False, False, True, False]) + + def test_large_batch_matches_scalar(self): + rng = np.random.default_rng(0) + targets = rng.uniform(low=[6, -3, 0], high=[15, 3, 4], size=(300, 3)) + batch = _quad_blocks_batch(self.OBS, targets, _UNIT_WALL) + for i, t in enumerate(targets): + self.assertEqual(bool(batch[i]), + _quad_blocks(tuple(self.OBS), tuple(t), _UNIT_WALL), # type: ignore[arg-type] + msg=f"mismatch at target {i}: {t}") + + +# --------------------------------------------------------------------------- +# Map sensor — grid scenarios +# --------------------------------------------------------------------------- + +class OccludedMapSensorTest(GridTest): + """ + Observer always at node (0,0) = position (0,0). + Sensor range 25 m covers rows 0–2 and cols 0–2 with no FOV restriction. + """ + + RANGE = 25.0 + + def _sense(self, label='occ'): + s = self.occluded_map(label, sensor_range=self.RANGE) + s.sense(self.nid(0, 0)) + return s.data + + def test_no_building_all_nodes_visible(self): + data = self._sense() + # Nodes within 25 m of (0,0): rows/cols 0–2 except the (2,2) corner + # which sits at distance √800 ≈ 28.3 m (outside range). + import math + for row in range(3): + for col in range(3): + dist = math.hypot(col * _GRID_SPACING, row * _GRID_SPACING) + if dist > self.RANGE: + continue # genuinely out of sensor range — skip + self.assertIn(self.nid(row, col), data['nodes'], + msg=f"node ({row},{col}) missing with no buildings") + + def test_building_blocks_column_ahead(self): + # Wall slab at x=5 (between col 0 and col 1), centred on y=0 axis. + # Blocks all nodes with col >= 1 and row == 0 when looking straight along +x. + self.add_building(4, -3, 6, 3) + data = self._sense() + self.assertIn(self.nid(0, 0), data['nodes']) # observer always visible + self.assertNotIn(self.nid(0, 1), data['nodes']) # directly behind wall + self.assertNotIn(self.nid(0, 2), data['nodes']) # further behind wall + + def test_building_does_not_block_perpendicular_nodes(self): + # Same wall along x=5 — nodes above (col 0, row 1+) have clear LOS. + self.add_building(4, -3, 6, 3) + data = self._sense() + self.assertIn(self.nid(1, 0), data['nodes']) + self.assertIn(self.nid(2, 0), data['nodes']) + + def test_building_beside_path_does_not_occlude(self): + # Building far off to the side (y > 15) — no ray to any in-range node crosses it. + self.add_building(3, 18, 7, 22) + data = self._sense() + for col in range(3): + self.assertIn(self.nid(0, col), data['nodes'], + msg=f"node (0,{col}) should be visible past a side building") + + def test_two_buildings_each_block_one_direction(self): + # Building A: blocks +x from origin (between col 0 and col 1). + self.add_building(4, -3, 6, 3) + # Building B: blocks +y from origin (between row 0 and row 1). + self.add_building(-3, 4, 3, 6) + data = self._sense() + self.assertIn(self.nid(0, 0), data['nodes']) + self.assertNotIn(self.nid(0, 1), data['nodes']) # blocked by A + self.assertNotIn(self.nid(1, 0), data['nodes']) # blocked by B + + def test_observer_node_always_in_output(self): + self.add_building(4, -3, 6, 3) + data = self._sense() + self.assertIn(self.nid(0, 0), data['nodes']) + + def test_edge_excluded_when_both_endpoints_hidden(self): + self.add_building(4, -3, 6, 3) + data = self._sense() + visible = set(data['nodes'].keys()) + for edge in data['edges']: + self.assertIn(edge.source, visible) + self.assertIn(edge.target, visible) + + def test_building_outside_range_not_loaded(self): + # Building at x=100 is far outside the 25 m sensor range — must be ignored. + self.add_building(99, -3, 101, 3) + data = self._sense() + # Nodes in col 1 and 2 should still be visible (no occluder in range). + self.assertIn(self.nid(0, 1), data['nodes']) + self.assertIn(self.nid(0, 2), data['nodes']) + + def test_tall_building_blocks_low_observer(self): + # 8 m wall easily blocks a 1.6 m observer. + self.add_building(4, -3, 6, 3, height=_WALL_HEIGHT) + data = self._sense() + self.assertNotIn(self.nid(0, 1), data['nodes']) + + def test_short_building_does_not_block_observer(self): + # A 1 m wall is shorter than the observer eye-level (1.6 m) — ray passes over. + self.add_building(4, -3, 6, 3, height=1.0) + data = self._sense() + self.assertIn(self.nid(0, 1), data['nodes']) + + def test_infinite_range_still_occludes(self): + self.add_building(4, -3, 6, 3) + s = self.occluded_map('occ_inf', sensor_range=float('inf')) + s.sense(self.nid(0, 0)) + self.assertNotIn(self.nid(0, 1), s.data['nodes']) + + def test_infinite_range_far_nodes_visible(self): + # No buildings — all 25 nodes reachable with infinite range. + s = self.occluded_map('occ_inf', sensor_range=float('inf')) + s.sense(self.nid(0, 0)) + self.assertEqual(len(s.data['nodes']), _GRID_N ** 2) + + +# --------------------------------------------------------------------------- +# Map sensor — FOV scenarios +# --------------------------------------------------------------------------- + +class OccludedMapFovTest(GridTest): + """ + FOV and occlusion compose: observer at (0,0), orientation=(1,0) (+x). + FOV = π/2 (90°) → ±45° half-cone. + + Nodes in the +x direction (row 0, col 1+) are inside the cone. + Nodes in the +y direction (row 1+, col 0) are outside the cone. + """ + + def _sense_fov(self, fov, label='occ_fov', **kwargs): + s = self.occluded_map(label, sensor_range=25.0, fov=fov, + orientation=(1.0, 0.0), **kwargs) + s.sense(self.nid(0, 0)) + return s.data + + def test_forward_node_inside_cone(self): + data = self._sense_fov(math.pi / 2) + self.assertIn(self.nid(0, 1), data['nodes']) + + def test_perpendicular_node_outside_cone(self): + # Node directly above (row 1, col 0) is at 90° — outside ±45° cone. + data = self._sense_fov(math.pi / 2) + self.assertNotIn(self.nid(1, 0), data['nodes']) + + def test_wall_blocks_forward_node_inside_cone(self): + self.add_building(4, -3, 6, 3) + data = self._sense_fov(math.pi / 2) + self.assertNotIn(self.nid(0, 1), data['nodes']) # wall occludes + + def test_full_fov_ignores_cone_keeps_wall(self): + # 2π FOV — cone is off; only the wall matters. + self.add_building(4, -3, 6, 3) + data = self._sense_fov(2 * math.pi) + self.assertNotIn(self.nid(0, 1), data['nodes']) # wall + self.assertIn(self.nid(1, 0), data['nodes']) # clear LOS, was outside cone + + def test_narrow_fov_cuts_diagonal_node(self): + # Node (1,1) is at 45° — right on the edge for fov=π/2 (half-angle 45°). + # fov=π/3 (60°, half-angle 30°) should cut it. + data = self._sense_fov(math.pi / 3) + self.assertNotIn(self.nid(1, 1), data['nodes']) + + +# --------------------------------------------------------------------------- +# Agent sensor — grid scenarios +# --------------------------------------------------------------------------- + +class OccludedAgentSensorTest(GridTest): + """Observer at node (0,0); agents placed at various grid nodes.""" + + def _add_agent(self, name, row, col): + self.ctx.agent.create_agent(name, start_node_id=self.nid(row, col)) + + def test_no_building_all_agents_visible(self): + self._add_agent('a1', 0, 1) + self._add_agent('a2', 1, 0) + s = self.occluded_agent(sensor_range=25.0) + s.sense(self.nid(0, 0)) + self.assertIn('a1', s.data) + self.assertIn('a2', s.data) + + def test_building_hides_agent_behind_it(self): + self.add_building(4, -3, 6, 3) + self._add_agent('hidden', 0, 1) + self._add_agent('visible', 1, 0) + s = self.occluded_agent(sensor_range=25.0) + s.sense(self.nid(0, 0)) + self.assertNotIn('hidden', s.data) + self.assertIn('visible', s.data) + + def test_agent_at_same_node_as_observer_visible(self): + self._add_agent('same_spot', 0, 0) + s = self.occluded_agent(sensor_range=25.0) + s.sense(self.nid(0, 0)) + self.assertIn('same_spot', s.data) + + def test_two_buildings_each_hide_one_agent(self): + self.add_building(4, -3, 6, 3) # blocks +x + self.add_building(-3, 4, 3, 6) # blocks +y + self._add_agent('hidden_x', 0, 2) + self._add_agent('hidden_y', 2, 0) + self._add_agent('visible', 1, 1) + s = self.occluded_agent(sensor_range=35.0) + s.sense(self.nid(0, 0)) + self.assertNotIn('hidden_x', s.data) + self.assertNotIn('hidden_y', s.data) + self.assertIn('visible', s.data) + + def test_agent_out_of_range_not_detected(self): + self._add_agent('far', 4, 4) + s = self.occluded_agent(sensor_range=15.0) + s.sense(self.nid(0, 0)) + self.assertNotIn('far', s.data) + + +# --------------------------------------------------------------------------- +# Aerial sensor — grid scenarios +# --------------------------------------------------------------------------- + +class OccludedAerialSensorTest(GridTest): + """Drone looks down at the grid; building occluded nodes from low altitude.""" + + def _make_drone(self, name, z): + drone = self.ctx.agent.create_agent( + name, + type=gamms.typing.agent_engine.AgentType.AERIAL, + start_node_id=self.nid(0, 0), + speed=5.0, + ) + drone.position = (0.0, 0.0, z) + return drone + + def test_drone_high_above_building_sees_all(self): + # Drone at 30 m — rays steep enough to clear the 8 m wall entirely. + self.add_building(4, -3, 6, 3) + drone = self._make_drone('drone', z=30.0) + s = self.occluded_aerial(sensor_range=60.0, fov=math.pi) + s.set_owner(drone.name) + s.sense(self.nid(0, 0)) + # High angle: node (0,1) should still be visible despite the wall. + self.assertIn(self.nid(0, 1), s.data['nodes']) + + def test_drone_at_eye_level_loses_node_behind_wall(self): + # Drone at 1 m — nearly horizontal sightline, 8 m wall blocks it. + self.add_building(4, -3, 6, 3) + drone = self._make_drone('drone_low', z=1.0) + s = self.occluded_aerial( + sensor_range=60.0, fov=math.pi, + quat=(math.sqrt(0.5), 0.0, math.sqrt(0.5), 0.0), + ) + s.set_owner(drone.name) + s.sense(self.nid(0, 0)) + self.assertNotIn(self.nid(0, 1), s.data['nodes']) + + def test_drone_side_node_always_visible(self): + # Node (1,0) is above the origin in y — the wall along x=5 never crosses this ray. + self.add_building(4, -3, 6, 3) + drone = self._make_drone('drone', z=5.0) + s = self.occluded_aerial(sensor_range=60.0, fov=math.pi) + s.set_owner(drone.name) + s.sense(self.nid(0, 0)) + self.assertIn(self.nid(1, 0), s.data['nodes']) + + +# --------------------------------------------------------------------------- +# Aerial agent sensor — grid scenarios +# --------------------------------------------------------------------------- + +class OccludedAerialAgentSensorTest(GridTest): + """Drone detecting ground agents through (or past) a building.""" + + def _make_drone(self, name, z): + drone = self.ctx.agent.create_agent( + name, + type=gamms.typing.agent_engine.AgentType.AERIAL, + start_node_id=self.nid(0, 0), + speed=5.0, + ) + drone.position = (0.0, 0.0, z) + return drone + + def _add_agent(self, name, row, col): + return self.ctx.agent.create_agent(name, start_node_id=self.nid(row, col)) + + def _sense(self, drone, label='occ_aa', **kwargs): + s = self.occluded_aerial_agent(label, sensor_range=60.0, + fov=math.pi, **kwargs) + s.set_owner(drone.name) + s.sense(self.nid(0, 0)) + return s.data + + def test_no_building_sees_all_agents(self): + self._add_agent('a1', 0, 1) + self._add_agent('a2', 1, 0) + drone = self._make_drone('drone', z=5.0) + data = self._sense(drone) + self.assertIn('a1', data) + self.assertIn('a2', data) + + def test_high_drone_sees_agent_behind_building(self): + self.add_building(4, -3, 6, 3) + self._add_agent('hidden_low', 0, 1) + drone = self._make_drone('drone_high', z=25.0) + data = self._sense(drone, label='occ_aa_high') + self.assertIn('hidden_low', data) + + def test_low_drone_loses_agent_behind_building(self): + self.add_building(4, -3, 6, 3) + self._add_agent('hidden', 0, 1) + drone = self._make_drone('drone_low', z=1.0) + data = self._sense(drone, label='occ_aa_low') + self.assertNotIn('hidden', data) + + def test_drone_never_detects_itself(self): + drone = self._make_drone('drone', z=5.0) + data = self._sense(drone) + self.assertNotIn('drone', data) + + def test_data_format_is_type_and_position(self): + self._add_agent('a1', 1, 1) + drone = self._make_drone('drone', z=5.0) + data = self._sense(drone) + self.assertIn('a1', data) + atype, pos = data['a1'] + self.assertEqual(len(pos), 3) + + +def suite(): + classes = [ + SegmentTriangleTest, + QuadBlocksTest, + QuadBlocksBatchTest, + OccludedMapSensorTest, + OccludedMapFovTest, + OccludedAgentSensorTest, + OccludedAerialSensorTest, + OccludedAerialAgentSensorTest, + ] + s = unittest.TestSuite() + for cls in classes: + s.addTests(unittest.defaultTestLoader.loadTestsFromTestCase(cls)) + return s + + +if __name__ == '__main__': + unittest.TextTestRunner().run(suite()) diff --git a/tests/test_sensor_decorator.py b/tests/test_sensor_decorator.py deleted file mode 100644 index bf18cc2..0000000 --- a/tests/test_sensor_decorator.py +++ /dev/null @@ -1,58 +0,0 @@ -import gamms -from gamms.typing.sensor_engine import SensorType, ISensor - -# --- Setup a minimal real context similar to game.py --- - - -# Create a gamms context. -ctx = gamms.create_context(vis_engine=gamms.visual.Engine.NO_VIS) - - - -# Obtain the sensor engine from the context. -sensor_engine = ctx.sensor - -# Define a custom sensor using the sensor_engine.custom decorator. -@sensor_engine.custom(name='TEST') -class CustomSensor(ISensor): - def __init__(self, extra_param=None): - # extra_param is just to demonstrate passing additional arguments. - self.extra_param = extra_param - - def sense(self, node_id: int) -> None: - # Minimal implementation for testing. - print(f"Sensing node: {node_id}") - - def set_owner(self, owner: str) -> None: - # Set the owner of the sensor. - self.owner = owner - - @property - def type(self) -> SensorType: - # Return the type of the sensor. - return SensorType.CUSTOM - - @property - def data(self): - return - - def update(self, data: dict) -> None: - print(f"Updating sensor with data: {data}") - -# --- Instantiate and test custom sensors --- - -# Create the first custom sensor instance with name "CustomA" -sensor1 = CustomSensor(extra_param=42) -# Create the second custom sensor instance with name "CustomB" -sensor2 = CustomSensor(extra_param=100) - -# Print the custom_data dictionary to verify initialization. -print("Sensor1 type:", sensor1.type) -print("Sensor2 type:", sensor2.type) - -# Optionally, exercise the sensor methods. -sensor1.sense(0) -sensor2.update({"sample": 123}) - -# Terminate the context. -ctx.terminate() diff --git a/tests/test_sensors.py b/tests/test_sensors.py deleted file mode 100644 index 92425a4..0000000 --- a/tests/test_sensors.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python3 -import gamms -import pickle -import math -from gamms.typing.sensor_engine import SensorType -from gamms.SensorEngine.sensor_engine import SensorEngine - -# --- Setup context, graph, sensors, and agents --- -ctx = gamms.create_context(vis_engine=gamms.visual.Engine.PYGAME) - -# Load the graph from file and attach it to the context. -with open("graph.pkl", 'rb') as f: - G = pickle.load(f) -ctx.graph.attach_networkx_graph(G) - -# Create 10 dummy agents for testing. -for i in range(10): - name = f"agent_{i}" - agent_config = { - 'meta': {'team': 0}, - 'sensors': [], - 'start_node_id': i # Each agent starts at a different node. - } - ctx.agent.create_agent(name, **agent_config) - -# Simulate movement for agent_0 to update its orientation. -# Initially, agent_0 starts at node 0; now we update it to node 1. -agent0 = ctx.agent.get_agent("agent_0") -agent0.current_node_id = 1 # This will update agent0.orientation - -# --- Create SensorEngine instance --- -sensor_engine = SensorEngine(ctx) - - -# --- Test NEIGHBOR Sensor --- -# neighbor_sensor = sensor_engine.create_sensor("neighbor_test", SensorType.NEIGHBOR) -# neighbor_sensor.sense(2) # Sense from node 0 -# print("=== NEIGHBOR Sensor Output ===") -# for edge in neighbor_sensor.data: -# print(f"{edge.id}, {edge.source}, {edge.target} ") - -# # --- Test MAP Sensor --- -# # MAP sensor: using MapSensor (via ArcSensor) with range=inf and fov=360 should detect all nodes. -# map_sensor = sensor_engine.create_sensor("map_test", SensorType.MAP) -# map_sensor.sense(0) # Sense from node 0 -# print("=== MAP Sensor Output ===") -# print("Nodes detected:", list(map_sensor.data.get('nodes', {}).keys())) -# (MAP sensor doesn't process agents) - -# # --- Test RANGE Sensor --- -# # RANGE sensor: using MapSensor with finite range (30) and fov=360. -range_sensor = sensor_engine.create_sensor("range_test", SensorType.RANGE, sensor_range=150) -range_sensor.set_owner("agent_0") -range_sensor.sense(0) # Sense from node 0 -print("=== RANGE Sensor Output ===") -print("Nodes detected:", list(range_sensor.data.get('nodes', {}).keys())) - -# # --- Test ARC Sensor --- -# # ARC sensor: using MapSensor with finite range (30) and a narrow fov (90). -# # Set the owner so that the sensor automatically uses agent_0's orientation. -arc_sensor = sensor_engine.create_sensor("arc_test", SensorType.ARC, sensor_range=150, fov=math.radians(90)) -range_sensor.set_owner("agent_0") -arc_sensor.sense(0) # Sense from node 0; will use agent_0.orientation -print("=== ARC Sensor Output ===") -print("Nodes detected:", list(arc_sensor.data.get('nodes', {}).keys())) - -# # --- Test AGENT Sensor (full FOV) --- -# # Agent sensor: detects agents within a 30-unit range (fov=360). -agent_sensor = sensor_engine.create_sensor("agent_test", SensorType.AGENT) -agent_sensor.owner = "agent_0" # Skip detecting agent_0 (the owner) -agent_sensor.sense(0) # Sense from node 0 -print("=== AGENT Sensor Output (Full FOV) ===") -print("Agents detected:", list(agent_sensor.data.keys())) - -# # --- Test AGENT_ARC Sensor (directional agent sensor) --- -# # Agent sensor with directional filtering: using fov=90. -agent_arc_sensor = sensor_engine.create_sensor("agent_arc_test", SensorType.AGENT_ARC) -agent_arc_sensor.owner = "agent_0" -agent_arc_sensor.sense(0) -print("=== AGENT_ARC Sensor Output (Directional) ===") -print("Agents detected:", list(agent_arc_sensor.data.keys())) - -# # --- Test AGENT_RANGE Sensor (full FOV agent sensor) --- -# # Agent sensor configured as full-range (fov=360). -agent_range_sensor = sensor_engine.create_sensor("agent_range_test", SensorType.AGENT_RANGE) -agent_range_sensor.owner = "agent_0" -agent_range_sensor.sense(0) -print("=== AGENT_RANGE Sensor Output ===") -print("Agents detected:", list(agent_range_sensor.data.keys())) - -ctx.terminate()