diff --git a/.coverage b/.coverage new file mode 100644 index 0000000..7d897fc Binary files /dev/null and b/.coverage differ diff --git a/.github/workflows/workflow.yml b/.github/workflows/workflow.yml index 7dd0949..f8cd10f 100644 --- a/.github/workflows/workflow.yml +++ b/.github/workflows/workflow.yml @@ -1,92 +1,107 @@ -name: colav_protobuf CI/CD -run-name: ${{ github.actor }} is testing colav_protobuf pypi pkg πŸš€ +name: riskenv CI/CD +run-name: ${{ github.actor }} β€” riskenv build, test & publish πŸš€ on: push: - branches: [main] + branches: [stable] pull_request: - branches: [main] + branches: [stable] release: types: [published] jobs: + # ------------------------------------------------------------------------- build: + # ------------------------------------------------------------------------- name: Build Package runs-on: ubuntu-latest steps: - - name: Checkout Repository - uses: actions/checkout@v4 + - uses: actions/checkout@v4 - name: Set up Python - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: - python-version: "3.10" + python-version: "3.11" - - name: Install Build Tools + - name: Install build tools run: | pip install --upgrade pip - pip install hatch protobuf>=5.29.3 + pip install hatch - - name: Build colav_protobuf Package + - name: Build riskenv run: hatch build - - name: Upload Build Artifacts + - name: Upload build artifacts uses: actions/upload-artifact@v4 with: name: dist-files path: dist/ + # ------------------------------------------------------------------------- test: + # ------------------------------------------------------------------------- name: Run Tests needs: build runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.10", "3.11", "3.12"] + steps: - - name: Checkout Repository - uses: actions/checkout@v4 + - uses: actions/checkout@v4 - - name: Set up Python - uses: actions/setup-python@v4 + - name: Set up Python ${{ matrix.python-version }} + uses: actions/setup-python@v5 with: - python-version: "3.10" + python-version: ${{ matrix.python-version }} - - name: Install Dependencies + - name: Install riskenv and test dependencies run: | pip install --upgrade pip - pip install -r requirements.txt - pip install hatch protobuf>=5.29.3 + pip install -e ".[test]" + pip install pytest pytest-cov + + - name: Run unit tests + run: | + python -m pytest tests/unit_tests/ -v --tb=short - # - name: Run Tests - # run: hatch test + - name: Run integration tests + run: | + python -m pytest tests/integration_tests/ -v --tb=short + - name: Upload coverage report + if: matrix.python-version == '3.11' + uses: actions/upload-artifact@v4 + with: + name: coverage-report + path: .coverage + + # ------------------------------------------------------------------------- deploy: - name: Publish to PyPI if Release + # ------------------------------------------------------------------------- + name: Publish to PyPI needs: test if: github.event_name == 'release' runs-on: ubuntu-latest + permissions: + id-token: write # required for PyPI trusted publishing + steps: - - name: Checkout Repository - uses: actions/checkout@v4 + - uses: actions/checkout@v4 - name: Set up Python - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: - python-version: "3.10" + python-version: "3.11" - - name: Download Build Artifacts + - name: Download build artifacts uses: actions/download-artifact@v4 with: name: dist-files path: dist/ - - name: Install Twine - run: pip install twine - - name: Publish to PyPI - env: - PYPI_USERNAME: __token__ - PYPI_PASSWORD: ${{ secrets.PYPI_API_TOKEN }} - run: | - twine upload dist/* -u "${PYPI_USERNAME}" -p "${PYPI_PASSWORD}" --non-interactive --verbose + uses: pypa/gh-action-pypi-publish@release/v1 diff --git a/.gitignore b/.gitignore index e981c8f..5ebb5d0 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ ./colav_unsafe_set_gen.egg-info/ ./.vscode/ **/__pycache__/ -**/.pytest_cache/ \ No newline at end of file +**/.pytest_cache/ +./.claude \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..2d95437 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,22 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/ryan/ros2_ws/install/hybraut_interfaces/include/**", + "/home/ryan/ros2_ws/install/colav_interfaces/include/**", + "/opt/ros/jazzy/include/**", + "/usr/include/**" + ], + "name": "ros2", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5e48b42 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,11 @@ +{ + "ROS2.distro": "jazzy", + "python.autoComplete.extraPaths": [ + "/home/ryan/ros2_ws/install/hybraut_tb3/lib/python3.12/site-packages", + "/home/ryan/ros2_ws/install/hybraut_ros2/lib/python3.12/site-packages", + "/home/ryan/ros2_ws/install/hybraut_nav/lib/python3.12/site-packages", + "/home/ryan/ros2_ws/install/hybraut_interfaces/lib/python3.12/site-packages", + "/home/ryan/ros2_ws/install/colav_interfaces/lib/python3.12/site-packages", + "/opt/ros/jazzy/lib/python3.12/site-packages" + ] +} \ No newline at end of file diff --git a/README.md b/README.md index 434dc78..ca8dd81 100644 --- a/README.md +++ b/README.md @@ -1,70 +1,122 @@ -# colav-unsafe-set +# riskenv -[![PyPI - Version](https://img.shields.io/pypi/v/colav-unsafe-set.svg)](https://pypi.org/project/colav-unsafe-set) -[![PyPI - Python Version](https://img.shields.io/pypi/pyversions/colav-unsafe-set.svg)](https://pypi.org/project/colav-unsafe-set) - -This package contains implementation of the custom risk assesment method for motion planners called unsafe set as defined in paper [geometric motion planning in dynamic environments](). -The following is the high level equation which this package implements. +[![PyPI - Version](https://img.shields.io/pypi/v/riskenv.svg)](https://pypi.org/project/riskenv) +[![PyPI - Python Version](https://img.shields.io/pypi/pyversions/riskenv.svg)](https://pypi.org/project/riskenv) +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](LICENSE) -![image](./docs/unsafe_set_calculation.png) +**riskenv** computes the **Risk Envelope** β€” a convex hull bounding all collision-risk regions around an agent given a set of moving obstacles. It is designed for real-time motion planning in dynamic environments and works in both 2D and 3D spatial contexts across any application domain (autonomous vessels, ground robots, UAVs, etc.). -Show image of geometric unsafe set: -![image](./docs/unsafe_set_diagram.png) +The Risk Envelope is defined by three filtering criteria (Indices of Interest I1, I2, I3) derived from closest-point-of-approach (CPA) geometry, and is returned as a set of convex hull vertices ready for use in a downstream planner. + +![Risk Envelope equation](./docs/unsafe_set_calculation.png) + +![Risk Envelope diagram](./docs/unsafe_set_diagram.png) ----- ## Table of Contents - [Installation](#installation) -- [Structure](#structure) - [Usage](#usage) +- [CPA metric summary](#cpa-metric-summary) +- [Structure](#structure) +- [References](#references) - [License](#license) ## Installation ```bash -pip install colav-unsafe-set +pip install riskenv ``` -risk assessment collision metrics summary +Requires Python β‰₯ 3.10. -| Case | v_rel_norm_sq | p_rel == [0, 0] | tcpa > 0 | DCPA | TCPA | -|-----------------------|---------------|-----------------|----------|------------------------------|---------------------------| -| 1.1 Identical position | < 1e-6 | βœ… | – | NaN | inf | -| 1.2 Stationary, offset | < 1e-6 | ❌ | – | β€–p_relβ€– | β€–p_relβ€– / β€–v1β€– (or inf) | -| 2.1 Future CPA | β‰₯ 1e-6 | – | βœ… | β€–p_rel + tcpa * v_relβ€– | computed tcpa | -| 2.2 CPA in past | β‰₯ 1e-6 | – | ❌ | NaN | NaN | +## Usage +### Minimal example -## Structure +```python +import math +from riskenv import create_unsafe_set, Agent, Obstacle + +agent = Agent( + position=(10.0, 10.0), # (x, y) or (x, y, z) in metres + heading=0.0, # yaw angle in radians + speed=15.0, # m/s + yaw_rate=0.2, # rad/s + safety_radius=5.0, # metres +) + +obstacles = [ + Obstacle( + position=(30.0, 20.0), + heading=math.pi, # facing along -x axis + speed=20.0, + yaw_rate=0.1, + safety_radius=10.0, + tag='vessel_a', + ), +] + +# dsf: distance safety factor (metres) β€” the proximity threshold for I1/I2/I3 +# time_of_interest: TCPA horizon in seconds for the I3 filter (default 15 s) +# Returns: list of [x, y] hull vertices, or [] if no risk region exists. +vertices = create_unsafe_set(agent=agent, obstacles=obstacles, dsf=10.0) +``` -## Usage -When pkg is installed, Using it is simple imports are as follows. +### Quaternion users + +Convert to a heading angle before constructing `Agent` or `Obstacle`: ```python +from riskenv import heading_from_quaternion + +heading = heading_from_quaternion(qx, qy, qz, qw) ``` +### Lower-level API -## Collaborators +All internal building blocks are importable directly from the top-level package: -This repository is developed and maintained by: +```python +from riskenv import ( + calc_cpa, # DCPA / TCPA for a single obstacle + calculate_obstacle_metrics_for_agent, # annotate all obstacles with CPA metrics + predict_position, # dead-reckoning position at time dt + calc_I1, calc_I2, calc_I3, # individual index-of-interest filters + unionise_indices_of_interest, # merge I1 / I2 / I3 without duplicates + gen_uIoI_convhull, # convex hull from a union set + ObstacleWithMetrics, # Obstacle + tcpa/dcpa container +) +``` -- **[Ryan McKee](https://github.com/rymc-dev)** -- **[Nikolaos Athanasopoulos](https://github.com/)** -- **[Wasif Naeem](https://github.com/)** +## CPA metric summary +| Case | v\_rel\_norm\_sq | p\_rel == [0, 0] | tcpa > 0 | DCPA | TCPA | +|---|---|---|---|---|---| +| 1.1 Identical position, same velocity | < 1e-6 | βœ… | – | NaN | inf | +| 1.2 Zero relative velocity, offset | < 1e-6 | ❌ | – | β€–p\_relβ€– | β€–p\_relβ€– / β€–v1β€– (or inf) | +| 2.1 Future CPA | β‰₯ 1e-6 | – | βœ… | β€–p\_rel + tcpa Β· v\_relβ€– | computed tcpa | +| 2.2 CPA in past or at t=0 | β‰₯ 1e-6 | – | ❌ | NaN | NaN | -## References +## Structure -This project provides a Python implementation of the unsafe set risk-assessment method for motion planning as defined in the following research work: +``` +src/riskenv/ +β”œβ”€β”€ objects/ β€” Agent, Obstacle, ObstacleWithMetrics +β”œβ”€β”€ risk_assessment/ β€” calc_cpa, heading_from_quaternion, calculate_obstacle_metrics_for_agent +β”œβ”€β”€ indices_of_interest/ β€” calc_I1, calc_I2, calc_I3, unionise_indices_of_interest +β”œβ”€β”€ collision_geometry/ β€” gen_uIoI_convhull +β”œβ”€β”€ position_prediction/ β€” predict_position +└── unsafe_set/ β€” create_unsafe_set (top-level orchestrator) +``` -### Primary Reference (Unsafe Set Definition) +## References -> A. Author(s), *β€œGeometric Motion Planning in Dynamic Environments,”* Journal/Conference, Year. -> *(Replace with full citation once available.)* +This package implements the Risk Envelope method for motion planning originally described in: -If the specific paper is published, provide the full citation (title, authors, venue, year), and I can format it in **IEEE**, **APA**, or **BibTeX** style. +> R. McKee, N. Athanasopoulos, W. Naeem, *"Geometric Motion Planning in Dynamic Environments"* β€” *(citation pending publication)* ## License -`colav-unsafe-set` is distributed under the terms of the [MIT](https://github.com/RyanMcKeeQUB/colav-unsafe-set/tree/main/LICENSE) license. +`riskenv` is distributed under the terms of the [MIT](LICENSE) license. diff --git a/pyproject.toml b/pyproject.toml index 940ab19..50ba8ec 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,69 +3,84 @@ requires = ["hatchling"] build-backend = "hatchling.build" [project] -name = "colav-unsafe-set" +name = "riskenv" dynamic = ["version"] -description = "A Python library for generating unsafe sets utilising the Indices of Interest." +description = "A Python library for computing Risk Envelopes β€” convex hulls of collision-risk regions for motion planning in dynamic environments." readme = "README.md" license = { file = "LICENSE" } authors = [ {name = "Ryan McKee", email = "r.mckee@qub.ac.uk"} ] classifiers = [ - "Development Status :: 4 - Beta", + "Development Status :: 5 - Production/Stable", "Programming Language :: Python", - "Programming Language :: Python :: 3.8", - "Programming Language :: Python :: 3.9", "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: Implementation :: CPython", "Programming Language :: Python :: Implementation :: PyPy", ] +requires-python = ">=3.10" dependencies = [ - "hatch", - "pyyaml", - "protobuf (<=3.20.1)", - "lark", + "numpy", + "scipy", "matplotlib", - "pandas", - "scipy" # Add this line +] + +[project.optional-dependencies] +test = [ + "pytest", + "pytest-cov", + "pyyaml", ] [project.urls] -Documentation = "https://github.com/RyanMcKeeQUB/colav-unsafe-set/blob/main/README.md" -Issues = "https://github.com/RyanMcKeeQUB/colav-unsafe-set/issues" -Source = "https://github.com/RyanMcKeeQUB/colav-unsafe-set" +Documentation = "https://github.com/RyanMcKeeQUB/riskenv/blob/stable/README.md" +Issues = "https://github.com/RyanMcKeeQUB/riskenv/issues" +Source = "https://github.com/RyanMcKeeQUB/riskenv" +Changelog = "https://github.com/RyanMcKeeQUB/riskenv/releases" [tool.hatch.version] path = "src/__about__.py" +[tool.hatch.build.targets.sdist] +packages = ["src/riskenv"] + +[tool.hatch.build.targets.wheel] +packages = ["src/riskenv"] + [tool.hatch.envs.types.scripts] check = "mypy --install-types --non-interactive {args:src/ tests}" [tool.hatch.envs.test] -dependencies = ["pytest","lark", "pyyaml", "hatch", "numpy", "scipy", "matplotlib", "pandas"] +dependencies = ["pytest", "pytest-cov", "pyyaml", "numpy", "scipy", "matplotlib"] -[tool.hatch.envs.test.types.scripts] -run = "pytest test/indices_of_interest/test_indices_of_interest.py" +[tool.hatch.envs.test.scripts] +unit = "python -m pytest tests/unit_tests/ -v --tb=short" +integration = "python -m pytest tests/integration_tests/ -v --tb=short" +all = "python -m pytest tests/ -v --tb=short" +cov = "python -m pytest tests/unit_tests/ --cov=riskenv --cov-report=term-missing" [tool.hatch.metadata] allow-direct-references = true -[tool.hatch.build.targets.sdist] -packages = ["src/colav_unsafe_set"] + +[tool.pytest.ini_options] +testpaths = ["tests"] +# Suppress ROS2 pytest plugins that are auto-discovered on systems with ROS installed. +addopts = "-p no:launch_testing -p no:launch_ros -p no:ament_pep257 -p no:ament_flake8 -p no:ament_xmllint -p no:ament_lint -p no:ament_copyright" [tool.coverage.run] -source_pkgs = ["colav_unsafe_set", "tests"] +source_pkgs = ["riskenv", "tests"] branch = true parallel = true omit = [ - "src/colav_unsafe_set/__about__.py", - "src/colav_unsafe_set/main.py" + "src/riskenv/__about__.py", + "src/riskenv/main.py" ] [tool.coverage.paths] -colav_protobuf = ["src/colav_unsafe_set"] -tests = ["tests", "*/colav-protobuf-utils/tests"] +riskenv = ["src/riskenv"] +tests = ["tests", "*/riskenv/tests"] [tool.coverage.report] exclude_lines = [ diff --git a/session.md b/session.md new file mode 100644 index 0000000..9ef3caa --- /dev/null +++ b/session.md @@ -0,0 +1 @@ +claude --resume c15be2c2-46eb-4fac-b9e8-da8192703760 diff --git a/src/__about__.py b/src/__about__.py index 75dd6fd..89c6086 100644 --- a/src/__about__.py +++ b/src/__about__.py @@ -1,2 +1,2 @@ # SPDX-License-Identifier: MIT -__version__ = "0.0.4" +__version__ = "1.0.0" diff --git a/src/colav_unsafe_set/__init__.py b/src/colav_unsafe_set/__init__.py deleted file mode 100644 index 7139767..0000000 --- a/src/colav_unsafe_set/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from colav_unsafe_set.unsafe_set import create_unsafe_set -from colav_unsafe_set.risk_assessment import calculate_obstacle_metrics_for_agent - -__all__ = [ - 'create_unsafe_set', - 'calculate_obstacle_metrics_for_agent' -] \ No newline at end of file diff --git a/src/colav_unsafe_set/collision_geometry/collision_geometry.py b/src/colav_unsafe_set/collision_geometry/collision_geometry.py deleted file mode 100644 index c621e80..0000000 --- a/src/colav_unsafe_set/collision_geometry/collision_geometry.py +++ /dev/null @@ -1,81 +0,0 @@ -import numpy as np -from typing import List, Sequence -from scipy.spatial import ConvexHull -from colav_unsafe_set.objects import DynamicObstacleWithMetrics -from colav_unsafe_set.position_prediction import predict_position - -def gen_uIoI_convhull(uIoI: List[DynamicObstacleWithMetrics]) -> List[List[float]]: - """ - Generate the convex hull points of the union of safety regions from a list of dynamic obstacles. - - Each dynamic obstacle's safety region is approximated as a circle (using _generate_circle_vertices). - The union of all vertices from these circles is then used to compute the convex hull, and the function - returns the coordinates of the hull's vertices. - - Args: - uIoI (List[DynamicObstacleWithMetrics]): A list of dynamic obstacles with associated metrics. - - Returns: - List[List[float]]: A list of coordinate pairs representing the convex hull vertices. - """ - # Collect vertices for current positions - unsafe_set_vertices: List[List[float]] = [ - vertex - for dynamic_obstacle in uIoI - for vertex in _generate_circle_vertices( - centroid=[ - dynamic_obstacle.dynamic_obstacle.position[0], - dynamic_obstacle.dynamic_obstacle.position[1] - ], - radius=dynamic_obstacle.dynamic_obstacle.safety_radius - ) - ] - - # Collect vertices for predicted future positions - for dynamic_obstacle in uIoI: - if dynamic_obstacle.tcpa > 0 and not np.isnan(dynamic_obstacle.tcpa): - future_position = predict_position( - position=dynamic_obstacle.dynamic_obstacle.position, - quaternion_orientation=dynamic_obstacle.dynamic_obstacle.orientation, - velocity=dynamic_obstacle.dynamic_obstacle.velocity, - yaw_rate=dynamic_obstacle.dynamic_obstacle.yaw_rate, - dt=dynamic_obstacle.tcpa - ) - # Use extend to avoid nested lists - unsafe_set_vertices.extend( - _generate_circle_vertices( - centroid=[future_position[0], future_position[1]], - radius=dynamic_obstacle.dynamic_obstacle.safety_radius - ) - ) - - if not unsafe_set_vertices: - return [] - - unsafe_set_vertices = np.array(unsafe_set_vertices, dtype=np.float64) # Ensure float type - hull_indices = ConvexHull(unsafe_set_vertices).vertices - hull_points = unsafe_set_vertices[hull_indices].tolist() # Convert back to a list of lists - - return hull_points - - -def _generate_circle_vertices(centroid: Sequence[float], radius: float, num_points: int = 10) -> List[List[float]]: - """ - Generate vertices approximating a circle in the XY plane. - - The circle is defined by its centroid and radius, and is approximated by 'num_points' evenly spaced vertices. - - Args: - centroid (Sequence[float]): The (x, y) coordinates of the circle's center. - radius (float): The radius of the circle. - num_points (int): The number of vertices to generate (default is 10). - - Returns: - List[List[float]]: A list of [x, y] vertices representing the circle. - """ - x_c, y_c = centroid - theta = np.linspace(0, 2 * np.pi, num_points, endpoint=False) - x = x_c + radius * np.cos(theta) - y = y_c + radius * np.sin(theta) - circle_vertices = np.column_stack((x, y)) - return circle_vertices.tolist() diff --git a/src/colav_unsafe_set/indices_of_interest/indices_of_interest.py b/src/colav_unsafe_set/indices_of_interest/indices_of_interest.py deleted file mode 100644 index 914a1b8..0000000 --- a/src/colav_unsafe_set/indices_of_interest/indices_of_interest.py +++ /dev/null @@ -1,69 +0,0 @@ -from colav_unsafe_set.objects import ( - Agent, - DynamicObstacle, - DynamicObstacleWithMetrics -) -from scipy.spatial.distance import euclidean -from typing import List - -def compute_agent_obstacle_distance(agent: Agent, obstacle: DynamicObstacleWithMetrics) -> float: - """ - Compute the adjusted Euclidean distance between an agent and a dynamic obstacle, - subtracting both their safety radii. - """ - return euclidean(agent.position, obstacle.dynamic_obstacle.position) - ( - agent.safety_radius + obstacle.dynamic_obstacle.safety_radius - ) - -def compute_obstacle_distance( - obstacle1: DynamicObstacleWithMetrics, obstacle2: DynamicObstacleWithMetrics -) -> float: - """ - Compute the adjusted Euclidean distance between two dynamic obstacles, - subtracting their safety radii. - """ - return euclidean(obstacle1.dynamic_obstacle.position, obstacle2.dynamic_obstacle.position) - ( - obstacle1.dynamic_obstacle.safety_radius + obstacle2.dynamic_obstacle.safety_radius - ) - -def calc_I1( - agent: Agent, - dynamic_obstacles_with_metrics: List[DynamicObstacleWithMetrics], - dsf: float, -) -> List[DynamicObstacleWithMetrics]: - """Calculate the set of obstacles that are within the distance safety threshold (dsf) from the agent.""" - return [ - obstacle - for obstacle in dynamic_obstacles_with_metrics - if compute_agent_obstacle_distance(agent, obstacle) <= dsf - ] - -def calc_I2( - I1: List[DynamicObstacleWithMetrics], - dynamic_obstacles_with_metrics: List[DynamicObstacleWithMetrics], - dsf: float, -) -> List[DynamicObstacleWithMetrics]: - """ - Calculate the set of obstacles from I1 that have at least one other dynamic obstacle - (from dynamic_obstacles_with_metrics) within the distance safety threshold. - """ - I2 = [] - for operand in I1: - for arg in dynamic_obstacles_with_metrics: - if operand == arg: - continue - if compute_obstacle_distance(operand, arg) <= dsf: - I2.append(operand) - break # add each operand only once and move to the next - return I2 - -def calc_I3( - dynamic_obstacles_with_metrics: List[DynamicObstacleWithMetrics], - dsf: float, - time_of_interest: float -) -> List[DynamicObstacleWithMetrics]: - """Calculate the set of obstacles whose DCPA at the time of TCPA is within the distance safety threshold.""" - return [ - dob for dob in dynamic_obstacles_with_metrics - if dob.dcpa <= dsf and dob.tcpa <= time_of_interest - ] \ No newline at end of file diff --git a/src/colav_unsafe_set/indices_of_interest/unionise_indices_of_interest.py b/src/colav_unsafe_set/indices_of_interest/unionise_indices_of_interest.py deleted file mode 100644 index e7a0808..0000000 --- a/src/colav_unsafe_set/indices_of_interest/unionise_indices_of_interest.py +++ /dev/null @@ -1,12 +0,0 @@ -from colav_unsafe_set.objects import DynamicObstacleWithMetrics -from typing import List - -def unionise_indices_of_interest( - I1: List[DynamicObstacleWithMetrics], - I2: List[DynamicObstacleWithMetrics], - I3: List[DynamicObstacleWithMetrics], -) -> List[DynamicObstacleWithMetrics]: - """Unionise the indices of interest by removing duplicates using object identity.""" - # Use a dict keyed by the object's id to eliminate duplicates. - union_dict = {id(item): item for item in (I1 + I2 + I3)} - return list(union_dict.values()) diff --git a/src/colav_unsafe_set/objects/__init__.py b/src/colav_unsafe_set/objects/__init__.py deleted file mode 100644 index 59d7d40..0000000 --- a/src/colav_unsafe_set/objects/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from .objects import ( - Agent, - DynamicObstacle, - DynamicObstacleWithMetrics -) - -__all__ = ['Agent', 'DynamicObstacle', 'DynamicObstacleWithMetrics'] \ No newline at end of file diff --git a/src/colav_unsafe_set/objects/objects.py b/src/colav_unsafe_set/objects/objects.py deleted file mode 100644 index 79455db..0000000 --- a/src/colav_unsafe_set/objects/objects.py +++ /dev/null @@ -1,28 +0,0 @@ -from dataclasses import dataclass -from typing import Tuple - -@dataclass -class Agent: - """Represents an agent with position, orientation, velocity, yaw rate, and safety radius.""" - position: Tuple[float, float, float] # Cartesian position (x, y, z) - orientation: Tuple[float, float, float, float] # Quaternion orientation (x, y, z, w) - velocity: float # Velocity in m/s - yaw_rate: float # Yaw rate in rad/s - safety_radius: float # Safety radius in meters - -@dataclass -class DynamicObstacle: - """Represents a dynamic obstacle with its kinematic properties and safety radius.""" - tag: str - position: Tuple[float, float, float] # Cartesian position (x, y, z) - orientation: Tuple[float, float, float, float] # Quaternion orientation (x, y, z, w) - velocity: float # Velocity in m/s - yaw_rate: float # Yaw rate in rad/s - safety_radius: float # Safety radius in meters - -@dataclass -class DynamicObstacleWithMetrics: - """Associates a dynamic obstacle with additional metrics like TCPA and DCPA.""" - dynamic_obstacle: DynamicObstacle - tcpa: float # Time to Closest Point of Approach - dcpa: float # Distance at Closest Point of Approach diff --git a/src/colav_unsafe_set/position_prediction/position_prediction.py b/src/colav_unsafe_set/position_prediction/position_prediction.py deleted file mode 100644 index 862efd7..0000000 --- a/src/colav_unsafe_set/position_prediction/position_prediction.py +++ /dev/null @@ -1,53 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation as R -from typing import Tuple - -def predict_position( - position: Tuple[float, ...], - quaternion_orientation: Tuple[float, float, float, float], - velocity: float, - yaw_rate: float, - dt: float -) -> np.ndarray: - """ - Predicts the future position of an object given its current state. - - Args: - position: Tuple[float, ...] - Current position as (x, y) or (x, y, z) - quaternion_orientation: Tuple[float, float, float, float] - Orientation as a quaternion - velocity: float - Speed in the current direction - yaw_rate: float - Change in yaw per second (rad/s) - dt: float - Time step for prediction - - Returns: - np.ndarray: Predicted position as [x_new, y_new, z_new] (3D if needed) - """ - # Convert position to an array and pad with 0 if needed. - pos_array = np.array(position, dtype=np.float64) - if pos_array.size == 2: - pos_array = np.concatenate([pos_array, np.array([0.0])]) # Add z=0 for 2D position - - # Ensure dt is a valid value (not NaN) - if np.isnan(dt) or dt <= 0: - raise ValueError("Time step (dt) must be a positive number") - - # Normalize the quaternion if it's not normalized - norm = np.linalg.norm(quaternion_orientation) - if norm != 1.0: - quaternion_orientation = tuple(np.array(quaternion_orientation) / norm) - - # Convert quaternion to Euler angles (yaw, pitch, roll) - r = R.from_quat(quaternion_orientation) - yaw, _, _ = r.as_euler('zyx', degrees=False) # yaw is the first Euler angle - - # Update yaw based on yaw_rate and dt - yaw_new = yaw + yaw_rate * dt - - # Compute displacement in the XY plane - dx = velocity * np.cos(yaw_new) * dt - dy = velocity * np.sin(yaw_new) * dt - - # Update position (assuming no change in z for 2D) - new_position = pos_array + np.array([dx, dy, 0.0]) - - return new_position diff --git a/src/colav_unsafe_set/risk_assessment/__init__.py b/src/colav_unsafe_set/risk_assessment/__init__.py deleted file mode 100644 index 8df796f..0000000 --- a/src/colav_unsafe_set/risk_assessment/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -from .risk_assessment import calc_cpa -from .obstacle_metric_calculator import calculate_obstacle_metrics_for_agent - -__all__ = ['calc_cpa', 'calculate_obstacle_metrics_for_agent'] \ No newline at end of file diff --git a/src/colav_unsafe_set/risk_assessment/obstacle_metric_calculator.py b/src/colav_unsafe_set/risk_assessment/obstacle_metric_calculator.py deleted file mode 100644 index 931b5e1..0000000 --- a/src/colav_unsafe_set/risk_assessment/obstacle_metric_calculator.py +++ /dev/null @@ -1,20 +0,0 @@ -from colav_unsafe_set.objects import ( - Agent, - DynamicObstacle, - DynamicObstacleWithMetrics -) -from .risk_assessment import calc_cpa -from typing import List - - -def calculate_obstacle_metrics_for_agent(agent_vessel: Agent, dynamic_obstacles: List[DynamicObstacle]) -> List[DynamicObstacleWithMetrics]: - """Iterates through DynamicObstacles calculating DCPA and TCPA relative to agent configuration""" - dynamic_obstacles_with_metrics = [] - for dynamic_obstacle in dynamic_obstacles: - [dcpa, tcpa] = calc_cpa(agent_vessel, dynamic_obstacle) - dynamic_obstacles_with_metrics.append( - DynamicObstacleWithMetrics( - dynamic_obstacle=dynamic_obstacle, dcpa=dcpa, tcpa=tcpa - ) - ) - return dynamic_obstacles_with_metrics diff --git a/src/colav_unsafe_set/risk_assessment/risk_assessment.py b/src/colav_unsafe_set/risk_assessment/risk_assessment.py deleted file mode 100644 index ed5bb2a..0000000 --- a/src/colav_unsafe_set/risk_assessment/risk_assessment.py +++ /dev/null @@ -1,56 +0,0 @@ -import numpy as np -import math -from typing import Tuple -from colav_unsafe_set.objects import Agent, DynamicObstacle - -def quaternion_to_heading(qx, qy, qz, qw) -> float: - """Convert quaternion to heading angle in radians.""" - norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2) - qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm - siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) - return math.atan2(siny_cosp, cosy_cosp) - -def normalize_angle(angle: float) -> float: - """Normalize angle to the range [-Ο€, Ο€].""" - return (angle + np.pi) % (2 * np.pi) - np.pi - -def calc_cpa(agent_object: Agent, target_object: DynamicObstacle) -> Tuple[float, float]: - """Calculate DCPA and TCPA between agent and target.""" - - # Positions - p1 = np.array(agent_object.position[:2]) - p2 = np.array(target_object.position[:2]) - - # Headings - theta1 = normalize_angle(quaternion_to_heading(*agent_object.orientation)) - theta2 = normalize_angle(quaternion_to_heading(*target_object.orientation)) - - # Velocity vectors - v1 = agent_object.velocity * np.array([np.cos(theta1), np.sin(theta1)]) - v2 = target_object.velocity * np.array([np.cos(theta2), np.sin(theta2)]) - - # Relative vectors - p_rel = p1 - p2 - v_rel = v1 - v2 - v_rel_norm_sq = np.dot(v_rel, v_rel) - - if v_rel_norm_sq < 1e-6: - if np.allclose(p_rel, [0, 0]): - dcpa = float('nan') - tcpa = float('inf') - else: - distance = np.linalg.norm(p_rel) - speed = np.linalg.norm(v1) - dcpa = distance - tcpa = distance / speed if speed > 0 else float('inf') - else: - tcpa = -np.dot(p_rel, v_rel) / v_rel_norm_sq - if tcpa > 0: - cpa_vector = p_rel + tcpa * (-v_rel) - dcpa = np.linalg.norm(cpa_vector) - else: - dcpa = float('nan') - tcpa = float('nan') - - return dcpa, tcpa diff --git a/src/colav_unsafe_set/unsafe_set/unsafe_set.py b/src/colav_unsafe_set/unsafe_set/unsafe_set.py deleted file mode 100644 index b9cf20f..0000000 --- a/src/colav_unsafe_set/unsafe_set/unsafe_set.py +++ /dev/null @@ -1,60 +0,0 @@ -from typing import List -from colav_unsafe_set.objects import Agent, DynamicObstacle -from colav_unsafe_set.indices_of_interest import calc_I1, calc_I2, calc_I3, unionise_indices_of_interest -from colav_unsafe_set.risk_assessment import calculate_obstacle_metrics_for_agent -from colav_unsafe_set.collision_geometry import gen_uIoI_convhull - -def create_unsafe_set( - agent: Agent, - dynamic_obstacles: List[DynamicObstacle], - dsf: float, -) -> List[int]: - """ - Create an unsafe set for an agent by computing obstacle metrics, determining indices - of interest, unionizing these indices, and generating a convex hull around the unsafe regions. - - The process involves: - 1. Calculating dynamic obstacle metrics (DCPA and TCPA) relative to the agent. - 2. Determining indices of interest (I1, I2, I3) based on the distance safety threshold (dsf). - 3. Unionizing these indices to form the unionized unsafe indices of interest (uIoI). - 4. Generating the convex hull from the unionized unsafe set. - - Args: - agent (Agent): The agent for which the unsafe set is to be computed. - dynamic_obstacles (List[DynamicObstacle]): A list of dynamic obstacles. - dsf (float): The distance safety threshold. - - Returns: - List[int]: A list of indices representing the vertices of the convex hull of the unsafe set. - Returns an empty list if no unsafe regions are found. - """ - # Calculate dynamic obstacle metrics (e.g., DCPA, TCPA) relative to the agent. - dynamic_obstacle_metrics = calculate_obstacle_metrics_for_agent( - agent_vessel=agent, dynamic_obstacles=dynamic_obstacles - ) - - # Compute indices of interest based on the safety threshold. - I1 = calc_I1( - agent=agent, - dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, - dsf=dsf, - ) - I2 = calc_I2( - I1=I1, - dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, - dsf=dsf, - ) - I3 = calc_I3( - dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, - dsf=dsf, - time_of_interest=15 - ) - - # Unionize the indices of interest. - uIoI = unionise_indices_of_interest(I1, I2, I3) - if not uIoI: - return [] - - # Generate and return the convex hull of the unsafe set. - return gen_uIoI_convhull(uIoI) - diff --git a/src/main.py b/src/main.py index 9a26931..673e879 100644 --- a/src/main.py +++ b/src/main.py @@ -1,40 +1,41 @@ -import sys -import os +"""Minimal usage example for riskenv.""" +import math +from riskenv import create_unsafe_set, Agent, Obstacle, heading_from_quaternion -sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))) -from colav_unsafe_set import create_unsafe_set def main(): - from colav_unsafe_set.objects import DynamicObstacle, Agent - agent = Agent( - position=(float(10), float(10), float(10)), - orientation=(float(0), float(0), float(0), float(1)), - velocity=float(15.0), - yaw_rate=float(0.2), - safety_radius=float(5) + position=(10.0, 10.0), # 2-D position (x, y) in metres + heading=0.0, # facing along +x axis + speed=15.0, # m/s + yaw_rate=0.2, # rad/s + safety_radius=5.0, # metres ) - dynamic_obstacles = [ - DynamicObstacle( - tag='obstacle 1', - position=(float(30), float(20), float(0)), - orientation=(float(0), float(0), float(0), float(1)), - velocity=float(20.0), - yaw_rate=float(0.1), - safety_radius=float(10.0) + + obstacles = [ + Obstacle( + position=(30.0, 20.0), + heading=math.pi, # facing along -x axis + speed=20.0, + yaw_rate=0.1, + safety_radius=10.0, + tag='vessel_a', + ), + Obstacle( + position=(5.0, 7.0), + heading=math.pi / 4, # 45 degrees + speed=10.0, + yaw_rate=0.1, + safety_radius=7.0, + tag='vessel_b', ), - DynamicObstacle( - tag='obstacle 1', - position=(float(5), float(7), float(0)), - orientation=(float(0), float(0), float(0), float(1)), - velocity=float(10.0), - yaw_rate=float(0.1), - safety_radius=float(7.0) - ) ] - vertices = create_unsafe_set(agent=agent, dynamic_obstacles=dynamic_obstacles, dsf=float(10)) - print(vertices) + # --- Quaternion users: convert heading before constructing Agent/Obstacle --- + # heading = heading_from_quaternion(qx, qy, qz, qw) + + vertices = create_unsafe_set(agent=agent, obstacles=obstacles, dsf=10.0) + print("Risk Envelope hull vertices:", vertices) if __name__ == "__main__": diff --git a/src/riskenv/__init__.py b/src/riskenv/__init__.py new file mode 100644 index 0000000..1e7da79 --- /dev/null +++ b/src/riskenv/__init__.py @@ -0,0 +1,48 @@ +from riskenv.unsafe_set import create_unsafe_set +from riskenv.risk_assessment import ( + calc_cpa, + calculate_obstacle_metrics_for_agent, + heading_from_quaternion, + quaternion_to_heading, # backward-compat alias +) +from riskenv.objects import ( + Agent, + Obstacle, + ObstacleWithMetrics, + # Backward-compat aliases + DynamicObstacle, + DynamicObstacleWithMetrics, +) +from riskenv.position_prediction import predict_position +from riskenv.indices_of_interest import ( + calc_I1, + calc_I2, + calc_I3, + unionise_indices_of_interest, +) +from riskenv.collision_geometry import gen_uIoI_convhull + +__all__ = [ + # Primary API + 'create_unsafe_set', + 'calculate_obstacle_metrics_for_agent', + # Data classes + 'Agent', + 'Obstacle', + 'ObstacleWithMetrics', + # Utilities + 'calc_cpa', + 'heading_from_quaternion', + 'predict_position', + # Index-of-interest filters + 'calc_I1', + 'calc_I2', + 'calc_I3', + 'unionise_indices_of_interest', + # Geometry + 'gen_uIoI_convhull', + # Backward-compat aliases (deprecated) + 'DynamicObstacle', + 'DynamicObstacleWithMetrics', + 'quaternion_to_heading', +] diff --git a/src/colav_unsafe_set/collision_geometry/__init__.py b/src/riskenv/collision_geometry/__init__.py similarity index 62% rename from src/colav_unsafe_set/collision_geometry/__init__.py rename to src/riskenv/collision_geometry/__init__.py index 69202d0..266b9fa 100644 --- a/src/colav_unsafe_set/collision_geometry/__init__.py +++ b/src/riskenv/collision_geometry/__init__.py @@ -1,3 +1,3 @@ from .collision_geometry import gen_uIoI_convhull -__all__ = ['gen_uIOI_convhull'] \ No newline at end of file +__all__ = ['gen_uIoI_convhull'] \ No newline at end of file diff --git a/src/riskenv/collision_geometry/collision_geometry.py b/src/riskenv/collision_geometry/collision_geometry.py new file mode 100644 index 0000000..71dd3c6 --- /dev/null +++ b/src/riskenv/collision_geometry/collision_geometry.py @@ -0,0 +1,54 @@ +import numpy as np +from typing import List, Sequence +from scipy.spatial import ConvexHull +from riskenv.objects import ObstacleWithMetrics +from riskenv.position_prediction import predict_position + + +def gen_uIoI_convhull(uIoI: List[ObstacleWithMetrics]) -> List[List[float]]: + """Generate the convex hull bounding all unsafe regions in the union of interest. + + Each obstacle's safety zone is approximated as a circle at its current position. + If a future CPA time is available, a second circle is added at the predicted position. + The convex hull of all circle vertices is returned. + + Args: + uIoI: Obstacles in the union of indices of interest (with CPA metrics). + + Returns: + List of [x, y] hull vertex coordinates, or [] if no vertices exist. + """ + vertices: List[List[float]] = [] + + for obs in uIoI: + o = obs.obstacle + vertices.extend(_circle_vertices(centroid=o.position[:2], radius=o.safety_radius)) + + if not np.isnan(obs.tcpa) and obs.tcpa > 0: + future_pos = predict_position( + position=o.position, + heading=o.heading, + speed=o.speed, + yaw_rate=o.yaw_rate, + dt=obs.tcpa, + ) + vertices.extend(_circle_vertices(centroid=future_pos[:2], radius=o.safety_radius)) + + if not vertices: + return [] + + pts = np.array(vertices, dtype=np.float64) + hull_indices = ConvexHull(pts).vertices + return pts[hull_indices].tolist() + + +def _circle_vertices( + centroid: Sequence[float], + radius: float, + num_points: int = 10, +) -> List[List[float]]: + """Approximate a circle as *num_points* evenly-spaced vertices.""" + x_c, y_c = centroid[0], centroid[1] + theta = np.linspace(0, 2 * np.pi, num_points, endpoint=False) + pts = np.column_stack((x_c + radius * np.cos(theta), y_c + radius * np.sin(theta))) + return pts.tolist() diff --git a/src/colav_unsafe_set/indices_of_interest/__init__.py b/src/riskenv/indices_of_interest/__init__.py similarity index 100% rename from src/colav_unsafe_set/indices_of_interest/__init__.py rename to src/riskenv/indices_of_interest/__init__.py diff --git a/src/riskenv/indices_of_interest/indices_of_interest.py b/src/riskenv/indices_of_interest/indices_of_interest.py new file mode 100644 index 0000000..ea98859 --- /dev/null +++ b/src/riskenv/indices_of_interest/indices_of_interest.py @@ -0,0 +1,61 @@ +import numpy as np +from riskenv.objects import Agent, ObstacleWithMetrics +from typing import List + + +def _agent_obstacle_distance(agent: Agent, obs: ObstacleWithMetrics) -> float: + """Euclidean distance between agent and obstacle, minus their combined safety radii.""" + p1 = np.array(agent.position[:2]) + p2 = np.array(obs.obstacle.position[:2]) + return float(np.linalg.norm(p1 - p2)) - (agent.safety_radius + obs.obstacle.safety_radius) + + +def _obstacle_obstacle_distance(a: ObstacleWithMetrics, b: ObstacleWithMetrics) -> float: + """Euclidean distance between two obstacles, minus their combined safety radii.""" + p1 = np.array(a.obstacle.position[:2]) + p2 = np.array(b.obstacle.position[:2]) + return float(np.linalg.norm(p1 - p2)) - (a.obstacle.safety_radius + b.obstacle.safety_radius) + + +def calc_I1( + agent: Agent, + dynamic_obstacles_with_metrics: List[ObstacleWithMetrics], + dsf: float, +) -> List[ObstacleWithMetrics]: + """Return obstacles currently within *dsf* of the agent (adjusted for safety radii).""" + return [ + obs for obs in dynamic_obstacles_with_metrics + if _agent_obstacle_distance(agent, obs) <= dsf + ] + + +def calc_I2( + I1: List[ObstacleWithMetrics], + dynamic_obstacles_with_metrics: List[ObstacleWithMetrics], + dsf: float, +) -> List[ObstacleWithMetrics]: + """Return obstacles from I1 that have at least one other obstacle within *dsf*.""" + result = [] + for obs in I1: + for other in dynamic_obstacles_with_metrics: + if obs is other: + continue + if _obstacle_obstacle_distance(obs, other) <= dsf: + result.append(obs) + break + return result + + +def calc_I3( + dynamic_obstacles_with_metrics: List[ObstacleWithMetrics], + dsf: float, + time_of_interest: float, +) -> List[ObstacleWithMetrics]: + """Return obstacles whose DCPA is within *dsf* and TCPA is within *time_of_interest*.""" + return [ + obs for obs in dynamic_obstacles_with_metrics + if not np.isnan(obs.dcpa) + and not np.isnan(obs.tcpa) + and obs.dcpa <= dsf + and obs.tcpa <= time_of_interest + ] diff --git a/src/riskenv/indices_of_interest/unionise_indices_of_interest.py b/src/riskenv/indices_of_interest/unionise_indices_of_interest.py new file mode 100644 index 0000000..ee84828 --- /dev/null +++ b/src/riskenv/indices_of_interest/unionise_indices_of_interest.py @@ -0,0 +1,11 @@ +from riskenv.objects import ObstacleWithMetrics +from typing import List + + +def unionise_indices_of_interest( + I1: List[ObstacleWithMetrics], + I2: List[ObstacleWithMetrics], + I3: List[ObstacleWithMetrics], +) -> List[ObstacleWithMetrics]: + """Merge I1, I2, and I3 into a single deduplicated list (by object identity).""" + return list({id(obs): obs for obs in I1 + I2 + I3}.values()) diff --git a/src/riskenv/objects/__init__.py b/src/riskenv/objects/__init__.py new file mode 100644 index 0000000..ca09b96 --- /dev/null +++ b/src/riskenv/objects/__init__.py @@ -0,0 +1,17 @@ +from .objects import ( + Agent, + Obstacle, + ObstacleWithMetrics, + # Backward-compat aliases + DynamicObstacle, + DynamicObstacleWithMetrics, +) + +__all__ = [ + 'Agent', + 'Obstacle', + 'ObstacleWithMetrics', + # Backward-compat aliases + 'DynamicObstacle', + 'DynamicObstacleWithMetrics', +] diff --git a/src/riskenv/objects/objects.py b/src/riskenv/objects/objects.py new file mode 100644 index 0000000..3363d40 --- /dev/null +++ b/src/riskenv/objects/objects.py @@ -0,0 +1,94 @@ +from dataclasses import dataclass, field +from typing import Tuple + + +@dataclass +class Agent: + """Represents the ego agent (vessel, robot, aircraft, etc.) in the environment. + + Attributes: + position: (x, y) or (x, y, z) coordinates in metres. + heading: Heading angle in radians (yaw, measured from the positive x-axis). + speed: Scalar speed in m/s. + yaw_rate: Angular rate around the vertical axis in rad/s. + safety_radius: Collision-avoidance radius in metres. + """ + position: Tuple[float, ...] + heading: float + speed: float + yaw_rate: float + safety_radius: float + + +@dataclass +class Obstacle: + """Represents any moving obstacle (vessel, robot, pedestrian, aircraft, etc.). + + Attributes: + position: (x, y) or (x, y, z) coordinates in metres. + heading: Heading angle in radians (yaw, measured from the positive x-axis). + speed: Scalar speed in m/s. + yaw_rate: Angular rate around the vertical axis in rad/s. + safety_radius: Collision-avoidance radius in metres. + tag: Optional identifier string. + """ + position: Tuple[float, ...] + heading: float + speed: float + yaw_rate: float + safety_radius: float + tag: str = "" + + +@dataclass +class ObstacleWithMetrics: + """An obstacle annotated with closest-point-of-approach metrics. + + Attributes: + obstacle: The underlying :class:`Obstacle`. + tcpa: Time to Closest Point of Approach in seconds (NaN if not applicable). + dcpa: Distance at Closest Point of Approach in metres (NaN if not applicable). + """ + obstacle: Obstacle + tcpa: float + dcpa: float + + +# --------------------------------------------------------------------------- +# Backward-compatibility aliases β€” these will be removed in a future release. +# --------------------------------------------------------------------------- + +def DynamicObstacle( + position: Tuple[float, ...], + heading: float = 0.0, + speed: float = 0.0, + yaw_rate: float = 0.0, + safety_radius: float = 0.0, + tag: str = "", + # Legacy quaternion keyword kept so old call-sites don't immediately crash. + orientation: Tuple[float, float, float, float] = None, + velocity: float = None, +) -> "Obstacle": + """Deprecated: use :class:`Obstacle` instead.""" + import warnings + warnings.warn( + "DynamicObstacle is deprecated; use Obstacle instead.", + DeprecationWarning, + stacklevel=2, + ) + if orientation is not None: + from riskenv.risk_assessment.risk_assessment import heading_from_quaternion + heading = heading_from_quaternion(*orientation) + if velocity is not None: + speed = velocity + return Obstacle( + position=position, + heading=heading, + speed=speed, + yaw_rate=yaw_rate, + safety_radius=safety_radius, + tag=tag, + ) + + +DynamicObstacleWithMetrics = ObstacleWithMetrics diff --git a/src/colav_unsafe_set/position_prediction/__init__.py b/src/riskenv/position_prediction/__init__.py similarity index 100% rename from src/colav_unsafe_set/position_prediction/__init__.py rename to src/riskenv/position_prediction/__init__.py diff --git a/src/riskenv/position_prediction/position_prediction.py b/src/riskenv/position_prediction/position_prediction.py new file mode 100644 index 0000000..d612c5d --- /dev/null +++ b/src/riskenv/position_prediction/position_prediction.py @@ -0,0 +1,36 @@ +import numpy as np +from typing import Tuple + + +def predict_position( + position: Tuple[float, ...], + heading: float, + speed: float, + yaw_rate: float, + dt: float, +) -> np.ndarray: + """Predict the future position of an object using a constant yaw-rate model. + + Args: + position: Current position as (x, y) or (x, y, z). + heading: Current heading in radians. + speed: Scalar speed in m/s. + yaw_rate: Angular rate in rad/s. + dt: Time step in seconds (must be positive). + + Returns: + Predicted position as a numpy array of the same length as *position*. + """ + if dt <= 0 or np.isnan(dt): + raise ValueError(f"dt must be a positive number, got {dt!r}") + + pos = np.array(position, dtype=np.float64) + yaw_new = heading + yaw_rate * dt + dx = speed * np.cos(yaw_new) * dt + dy = speed * np.sin(yaw_new) * dt + + displacement = np.zeros_like(pos) + displacement[0] = dx + displacement[1] = dy + + return pos + displacement diff --git a/src/riskenv/risk_assessment/__init__.py b/src/riskenv/risk_assessment/__init__.py new file mode 100644 index 0000000..c07ee32 --- /dev/null +++ b/src/riskenv/risk_assessment/__init__.py @@ -0,0 +1,9 @@ +from .risk_assessment import calc_cpa, heading_from_quaternion, quaternion_to_heading +from .obstacle_metric_calculator import calculate_obstacle_metrics_for_agent + +__all__ = [ + 'calc_cpa', + 'heading_from_quaternion', + 'quaternion_to_heading', + 'calculate_obstacle_metrics_for_agent', +] diff --git a/src/riskenv/risk_assessment/obstacle_metric_calculator.py b/src/riskenv/risk_assessment/obstacle_metric_calculator.py new file mode 100644 index 0000000..6a9ee49 --- /dev/null +++ b/src/riskenv/risk_assessment/obstacle_metric_calculator.py @@ -0,0 +1,23 @@ +from riskenv.objects import Agent, Obstacle, ObstacleWithMetrics +from .risk_assessment import calc_cpa +from typing import List + + +def calculate_obstacle_metrics_for_agent( + agent: Agent, + obstacles: List[Obstacle], +) -> List[ObstacleWithMetrics]: + """Compute DCPA and TCPA for every obstacle relative to *agent*. + + Args: + agent: The ego agent. + obstacles: List of obstacles to assess. + + Returns: + A list of :class:`ObstacleWithMetrics` with DCPA and TCPA populated. + """ + return [ + ObstacleWithMetrics(obstacle=obs, dcpa=dcpa, tcpa=tcpa) + for obs in obstacles + for dcpa, tcpa in [calc_cpa(agent, obs)] + ] diff --git a/src/riskenv/risk_assessment/risk_assessment.py b/src/riskenv/risk_assessment/risk_assessment.py new file mode 100644 index 0000000..6235791 --- /dev/null +++ b/src/riskenv/risk_assessment/risk_assessment.py @@ -0,0 +1,56 @@ +import numpy as np +import math +from typing import Tuple +from riskenv.objects import Agent, Obstacle + + +def heading_from_quaternion(qx: float, qy: float, qz: float, qw: float) -> float: + """Convert a unit quaternion to a heading (yaw) angle in radians. + + Normalises the quaternion before conversion so unnormalised inputs are handled safely. + """ + norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2) + qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm + siny_cosp = 2.0 * (qw * qz + qx * qy) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + return math.atan2(siny_cosp, cosy_cosp) + + +# Keep the old name as an alias. +quaternion_to_heading = heading_from_quaternion + + +def calc_cpa(agent: Agent, obstacle: Obstacle) -> Tuple[float, float]: + """Calculate DCPA and TCPA between *agent* and *obstacle*. + + Returns: + (dcpa, tcpa) β€” distance and time to closest point of approach. + Either value may be NaN when the CPA lies in the past or the geometry + is degenerate (identical positions, zero relative velocity, etc.). + """ + p1 = np.array(agent.position[:2], dtype=np.float64) + p2 = np.array(obstacle.position[:2], dtype=np.float64) + + v1 = agent.speed * np.array([np.cos(agent.heading), np.sin(agent.heading)]) + v2 = obstacle.speed * np.array([np.cos(obstacle.heading), np.sin(obstacle.heading)]) + + p_rel = p1 - p2 + v_rel = v1 - v2 + v_rel_norm_sq = np.dot(v_rel, v_rel) + + if v_rel_norm_sq < 1e-6: + if np.allclose(p_rel, [0, 0]): + return float('nan'), float('inf') + distance = np.linalg.norm(p_rel) + speed = np.linalg.norm(v1) + dcpa = distance + tcpa = distance / speed if speed > 0 else float('inf') + else: + tcpa = -np.dot(p_rel, v_rel) / v_rel_norm_sq + if tcpa > 0: + cpa_vector = p_rel + tcpa * v_rel + dcpa = np.linalg.norm(cpa_vector) + else: + return float('nan'), float('nan') + + return dcpa, tcpa diff --git a/src/colav_unsafe_set/unsafe_set/__init__.py b/src/riskenv/unsafe_set/__init__.py similarity index 100% rename from src/colav_unsafe_set/unsafe_set/__init__.py rename to src/riskenv/unsafe_set/__init__.py diff --git a/src/riskenv/unsafe_set/unsafe_set.py b/src/riskenv/unsafe_set/unsafe_set.py new file mode 100644 index 0000000..b9b2f0c --- /dev/null +++ b/src/riskenv/unsafe_set/unsafe_set.py @@ -0,0 +1,41 @@ +from typing import List +from riskenv.objects import Agent, Obstacle +from riskenv.indices_of_interest import calc_I1, calc_I2, calc_I3, unionise_indices_of_interest +from riskenv.risk_assessment import calculate_obstacle_metrics_for_agent +from riskenv.collision_geometry import gen_uIoI_convhull + + +def create_unsafe_set( + agent: Agent, + obstacles: List[Obstacle], + dsf: float, + time_of_interest: float = 15.0, +) -> List[List[float]]: + """Compute the Risk Envelope for *agent* given a list of obstacles. + + The Risk Envelope is the convex hull bounding all collision-risk regions that + the agent should avoid, determined by current proximity (I1, I2) and predicted + closest-point-of-approach geometry (I3). + + Args: + agent: The ego agent. + obstacles: Moving obstacles in the environment. + dsf: Distance safety factor in metres β€” the proximity threshold + used across all three index-of-interest criteria. + time_of_interest: Horizon in seconds for the I3 (TCPA-based) filter. Default 15. + + Returns: + List of [x, y] coordinates of the convex hull vertices bounding the unsafe + set. Returns [] when no obstacles pose a risk within the given parameters. + """ + metrics = calculate_obstacle_metrics_for_agent(agent=agent, obstacles=obstacles) + + I1 = calc_I1(agent=agent, dynamic_obstacles_with_metrics=metrics, dsf=dsf) + I2 = calc_I2(I1=I1, dynamic_obstacles_with_metrics=metrics, dsf=dsf) + I3 = calc_I3(dynamic_obstacles_with_metrics=metrics, dsf=dsf, time_of_interest=time_of_interest) + + uIoI = unionise_indices_of_interest(I1, I2, I3) + if not uIoI: + return [] + + return gen_uIoI_convhull(uIoI) diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..5d820f1 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,55 @@ +"""Shared pytest fixtures for riskenv tests.""" +import math +import pytest +from riskenv import Agent, Obstacle, ObstacleWithMetrics + + +@pytest.fixture +def agent(): + """Agent at the origin, heading east at 10 m/s.""" + return Agent( + position=(0.0, 0.0), + heading=0.0, + speed=10.0, + yaw_rate=0.0, + safety_radius=1.0, + ) + + +@pytest.fixture +def nearby_obstacle(): + """Obstacle 3 m east of the origin β€” well within a dsf of 5.""" + return Obstacle( + position=(3.0, 0.0), + heading=math.pi, + speed=5.0, + yaw_rate=0.0, + safety_radius=1.0, + tag="nearby", + ) + + +@pytest.fixture +def far_obstacle(): + """Obstacle 50 m east β€” outside a dsf of 5.""" + return Obstacle( + position=(50.0, 0.0), + heading=math.pi, + speed=5.0, + yaw_rate=0.0, + safety_radius=1.0, + tag="far", + ) + + +def make_obs_with_metrics(position, dcpa, tcpa, speed=5.0, safety_radius=1.0, tag="obs"): + """Helper: build an ObstacleWithMetrics with given CPA values.""" + obs = Obstacle( + position=position, + heading=0.0, + speed=speed, + yaw_rate=0.0, + safety_radius=safety_radius, + tag=tag, + ) + return ObstacleWithMetrics(obstacle=obs, dcpa=dcpa, tcpa=tcpa) diff --git a/tests/integration_tests/unsafe_set_integration_tests_visualisations.py b/tests/integration_tests/unsafe_set_integration_tests_visualisations.py index 340ac97..271229f 100644 --- a/tests/integration_tests/unsafe_set_integration_tests_visualisations.py +++ b/tests/integration_tests/unsafe_set_integration_tests_visualisations.py @@ -1,279 +1,306 @@ -import sys -import os +"""Integration tests for riskenv β€” scenario-driven visualisation and animation. + +Each YAML scenario file in ``scenarios/`` is loaded, run through +``create_unsafe_set``, and saved as an animated GIF under ``plots/``. + +YAML format +----------- +Scenarios may use either the current flat format:: + + agent_vessel: + position: [x, y] + heading: + speed: + yaw_rate: + safety_radius: + +or the legacy quaternion format (converted automatically):: + + agent_vessel: + position: [x, y] + orientation: [w, x, y, z] # quaternion + velocity: + yaw_rate: + safety_radius: +""" +from __future__ import annotations + +import math +from datetime import datetime from pathlib import Path -sys.path.append(os.path.abspath(__package__) + '/src') -from colav_unsafe_set.unsafe_set.unsafe_set import create_unsafe_set + +import matplotlib.gridspec as gridspec +import matplotlib.patches as patches +import matplotlib.pyplot as plt +import numpy as np import pytest import yaml -import os -from pprint import pprint # For better readability of printed YAML data -import matplotlib -import numpy as np -import pandas as pd -import matplotlib.pyplot as plt -plt.style.use('default') -from colav_unsafe_set.objects import Agent, DynamicObstacle -import matplotlib.patches as patches -from scipy.spatial.transform import Rotation as R from matplotlib.animation import FuncAnimation -# Set matplotlib style -plt.style.use('default') - -SCENARIO_DIR = Path(__file__).resolve().parent / 'scenarios' -PLOT_DIR = Path(__file__).resolve().parent / 'plots' -PLOT_DIR.mkdir(exist_ok=True) # Ensure plots directory exists - -def get_scenario_files(): - """Retrieve a list of YAML scenario files.""" - return [f for f in SCENARIO_DIR.iterdir() if f.suffix == '.yml'] - -def load_yaml(file_path): - """Load YAML content from a file.""" - with open(file_path, 'r') as file: - return yaml.safe_load(file) - -def quaternion_to_yaw(quaternion): - """Convert a quaternion (w, x, y, z) to a yaw angle in radians.""" - r = R.from_quat([quaternion[1], quaternion[2], quaternion[3], quaternion[0]]) # (x, y, z, w) - return r.as_euler('xyz', degrees=False)[2] # Yaw (rotation about Z-axis) - -# def quaternion_to_yaw(q): -# """Convert quaternion (w, x, y, z) to yaw (heading) angle in radians.""" -# w, x, y, z = q -# return np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2)) - -def normalize_angle(angle): - """Normalize angle to the range [-Ο€, Ο€].""" - return (angle + np.pi) % (2 * np.pi) - np.pi - -def compute_triangle_vertices(position, orientation, length=5, width=2): - """Compute vertices of an elongated triangle for a given position and orientation.""" - yaw = normalize_angle(quaternion_to_yaw(orientation)) - # Define the triangle in the local frame: tip at (length, 0) and base at (0, Β±width/2) - triangle = np.array([ - [length, 0], - [0, -width / 2], - [0, width / 2] - ]) - # Build the rotation matrix from the yaw angle - rotation_matrix = np.array([ - [np.cos(yaw), -np.sin(yaw)], - [np.sin(yaw), np.cos(yaw)] - ]) - # Rotate the triangle vertices and then translate to the world position - return (rotation_matrix @ triangle.T).T + position - -def draw_elongated_triangle(ax, position, orientation, length=5, width=2): - """Draw and return an elongated triangle representing an object at a given position and orientation.""" - vertices = compute_triangle_vertices(position, orientation, length, width) - triangle_patch = patches.Polygon(vertices, closed=True, facecolor='black') - ax.add_patch(triangle_patch) - return triangle_patch - -def update_orientation(orientation, yaw_rate): - """ - Update the orientation (heading) based on the yaw_rate and timestep. - Assuming the timestep is 1 frame (you can adjust this as needed). - """ - # Convert quaternion to yaw, add yaw rate, and convert back to quaternion - yaw = normalize_angle(quaternion_to_yaw(orientation)) - new_yaw = yaw + yaw_rate # Update yaw by yaw_rate - return yaw_to_quaternion(new_yaw) - -def quaternion_to_yaw(orientation): - """ - Convert quaternion orientation to yaw angle (in radians). - Assumes the orientation is a unit quaternion: [w, x, y, z]. - """ - # Extract the yaw from quaternion (assuming it is in the form [w, x, y, z]) - w, x, y, z = orientation - sin_yaw = 2 * (w * z + x * y) - cos_yaw = 1 - 2 * (y ** 2 + z ** 2) - yaw = np.arctan2(sin_yaw, cos_yaw) - return yaw - -def yaw_to_quaternion(yaw): - """ - Convert yaw angle to quaternion [w, x, y, z]. - """ - qw = np.cos(yaw / 2) - qx = 0 - qy = 0 - qz = np.sin(yaw / 2) - return np.array([qw, qx, qy, qz]) - -import matplotlib.gridspec as gridspec - -def plot_scenario(agent, obstacles, width, height, scenario_name, dsf, animate=False): - """Generate and optionally animate a plot of the scenario.""" - # Create a figure with a specified size (width x height in inches) +from riskenv import ( + Agent, + Obstacle, + create_unsafe_set, + heading_from_quaternion, + calculate_obstacle_metrics_for_agent, + calc_I1, + calc_I2, + calc_I3, + unionise_indices_of_interest, +) + +plt.style.use("default") + +SCENARIO_DIR = Path(__file__).resolve().parent / "scenarios" +PLOT_DIR = Path(__file__).resolve().parent / "plots" +PLOT_DIR.mkdir(exist_ok=True) + + +# --------------------------------------------------------------------------- +# YAML helpers +# --------------------------------------------------------------------------- + +def _load_yaml(path: Path) -> dict: + with open(path) as f: + return yaml.safe_load(f) + + +def _agent_from_dict(d: dict) -> Agent: + """Build an Agent from a scenario dict, handling both new and legacy fields.""" + pos = np.array(d["position"], dtype=float) + heading = _resolve_heading(d) + speed = float(d.get("speed", d.get("velocity", 0.0))) + return Agent( + position=pos, + heading=heading, + speed=speed, + yaw_rate=float(d.get("yaw_rate", 0.0)), + safety_radius=float(d.get("safety_radius", 0.0)), + ) + + +def _obstacle_from_dict(d: dict) -> Obstacle: + """Build an Obstacle from a scenario dict, handling both new and legacy fields.""" + pos = np.array(d["position"], dtype=float) + heading = _resolve_heading(d) + speed = float(d.get("speed", d.get("velocity", 0.0))) + return Obstacle( + position=pos, + heading=heading, + speed=speed, + yaw_rate=float(d.get("yaw_rate", 0.0)), + safety_radius=float(d.get("safety_radius", 0.0)), + tag=str(d.get("tag", "")), + ) + + +def _resolve_heading(d: dict) -> float: + """Return heading in radians from either ``heading`` or legacy ``orientation`` key.""" + if "heading" in d: + return float(d["heading"]) + if "orientation" in d: + ori = d["orientation"] + # Legacy format: [w, x, y, z] + return heading_from_quaternion(ori[1], ori[2], ori[3], ori[0]) + return 0.0 + + +def get_scenario_files() -> list[Path]: + return sorted(SCENARIO_DIR.glob("*.yml")) + + +# --------------------------------------------------------------------------- +# Visualisation helpers +# --------------------------------------------------------------------------- + +def _triangle_vertices( + position: np.ndarray, heading: float, length: float = 5.0, width: float = 2.0 +) -> np.ndarray: + """Compute elongated-triangle vertices for a given position and heading.""" + local = np.array([[length, 0.0], [0.0, -width / 2], [0.0, width / 2]]) + rot = np.array([[math.cos(heading), -math.sin(heading)], + [math.sin(heading), math.cos(heading)]]) + return (rot @ local.T).T + position[:2] + + +def _draw_triangle(ax, position: np.ndarray, heading: float, **kw) -> patches.Polygon: + verts = _triangle_vertices(position, heading) + patch = patches.Polygon(verts, closed=True, facecolor="black", **kw) + ax.add_patch(patch) + return patch + + +# --------------------------------------------------------------------------- +# Core test / visualisation +# --------------------------------------------------------------------------- + +def plot_scenario( + agent: Agent, + obstacles: list[Obstacle], + width: float, + height: float, + scenario_name: str, + dsf: float, + animate: bool = False, +) -> None: fig = plt.figure(figsize=(12, 6)) - - # Define a grid with 1 row and 2 columns, using width ratios so that the plot is larger. gs = gridspec.GridSpec(1, 2, width_ratios=[3, 1]) - - # Main animation plot on the left panel. + ax = fig.add_subplot(gs[0]) - half_width = width / 2 - half_height = height / 2 - ax.set_xlim(-half_width, half_width) - ax.set_ylim(-half_height, half_height) - ax.grid(color='black', linestyle='--', linewidth=0.5) - ax.set_xticks(np.arange(-half_width, half_width + 1, 50)) - ax.set_yticks(np.arange(-half_height, half_height + 1, 50)) - ax.set_xlabel('X Position') - ax.set_ylabel('Y Position') - ax.set_facecolor('white') - fig.patch.set_facecolor('white') - - # Metrics panel on the right panel. - metrics_ax = fig.add_subplot(gs[1]) - metrics_ax.axis('off') # Hide axis lines for the metrics panel + hw, hh = width / 2, height / 2 + ax.set_xlim(-hw, hw) + ax.set_ylim(-hh, hh) + ax.grid(color="black", linestyle="--", linewidth=0.5) + ax.set_xticks(np.arange(-hw, hw + 1, 50)) + ax.set_yticks(np.arange(-hh, hh + 1, 50)) + ax.set_xlabel("X Position (m)") + ax.set_ylabel("Y Position (m)") + ax.set_facecolor("white") + fig.patch.set_facecolor("white") - # Create a text object in the metrics panel. + metrics_ax = fig.add_subplot(gs[1]) + metrics_ax.axis("off") metrics_text = metrics_ax.text(0, 0.95, "", transform=metrics_ax.transAxes, - va='top', fontsize=6, family='monospace') - - # Plot agent and obstacles at the initial positions. - agent_circle = ax.add_patch(plt.Circle(agent.position, agent.safety_radius, - facecolor='blue', alpha=0.8, edgecolor='black')) - agent_triangle = draw_elongated_triangle(ax, agent.position, agent.orientation) - - obstacle_circles = [] - obstacle_triangles = [] - for obstacle in obstacles: - obstacle_circle = ax.add_patch(plt.Circle(obstacle.position, obstacle.safety_radius, - facecolor='red', alpha=0.5, edgecolor='black')) - triangle_patch = draw_elongated_triangle(ax, obstacle.position, obstacle.orientation) - obstacle_circles.append(obstacle_circle) - obstacle_triangles.append(triangle_patch) + va="top", fontsize=6, family="monospace") - def update(frame): - # Update agent orientation and position. - agent.orientation = update_orientation(agent.orientation, agent.yaw_rate) - delta_agent = np.array([ - np.cos(normalize_angle(quaternion_to_yaw(agent.orientation))) * agent.velocity, - np.sin(normalize_angle(quaternion_to_yaw(agent.orientation))) * agent.velocity - ]) - agent.position += delta_agent - agent_circle.center = agent.position - - new_agent_vertices = compute_triangle_vertices(agent.position, agent.orientation) - agent_triangle.set_xy(new_agent_vertices) - - # Update obstacles. - for i, obstacle in enumerate(obstacles): - obstacle.orientation = update_orientation(obstacle.orientation, obstacle.yaw_rate) - delta_obs = np.array([ - np.cos(normalize_angle(quaternion_to_yaw(obstacle.orientation))) * obstacle.velocity, - np.sin(normalize_angle(quaternion_to_yaw(obstacle.orientation))) * obstacle.velocity - ]) - obstacle.position += delta_obs - obstacle_circles[i].center = obstacle.position + # Initial patches + agent_pos = np.array(agent.position[:2], dtype=float) + agent_circle = ax.add_patch(plt.Circle(agent_pos, agent.safety_radius, + facecolor="blue", alpha=0.8, edgecolor="black")) + agent_tri = _draw_triangle(ax, agent_pos, agent.heading) - new_obs_vertices = compute_triangle_vertices(obstacle.position, obstacle.orientation) - obstacle_triangles[i].set_xy(new_obs_vertices) + obs_circles, obs_tris = [], [] + obs_positions = [np.array(o.position[:2], dtype=float) for o in obstacles] + obs_headings = [o.heading for o in obstacles] - # Compute metrics. - from colav_unsafe_set.risk_assessment import calculate_obstacle_metrics_for_agent - dynamic_obstacle_metrics = calculate_obstacle_metrics_for_agent(agent, obstacles) + for i, obs in enumerate(obstacles): + c = ax.add_patch(plt.Circle(obs_positions[i], obs.safety_radius, + facecolor="red", alpha=0.5, edgecolor="black")) + t = _draw_triangle(ax, obs_positions[i], obs_headings[i]) + obs_circles.append(c) + obs_tris.append(t) - from colav_unsafe_set.indices_of_interest import calc_I1, calc_I2, calc_I3, unionise_indices_of_interest - I1 = calc_I1(agent=agent, dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, dsf=dsf) - I2 = calc_I2(I1=I1, dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, dsf=dsf) - I3 = calc_I3(dynamic_obstacles_with_metrics=dynamic_obstacle_metrics, dsf=dsf, time_of_interest=15) - uIoI = unionise_indices_of_interest(I1, I2, I3) + unsafe_patch: patches.Polygon | None = None + + def update(frame): + nonlocal agent_pos, obs_positions, obs_headings, unsafe_patch - # Generate obstacle metadata string - obstacle_metadata = "\n".join([ - ( - f"Obstacle Tag: {obstacle.dynamic_obstacle.tag}\n" - f"Position: {obstacle.dynamic_obstacle.position}\n" - f"Orientation: {normalize_angle(quaternion_to_yaw(obstacle.dynamic_obstacle.orientation))}\n" - f"Velocity: {obstacle.dynamic_obstacle.velocity}\n" - f"yaw_rate: {obstacle.dynamic_obstacle.yaw_rate}\n" - f"TCPA: {obstacle.tcpa:.2f}\n" - f"DCPA: {obstacle.dcpa:.2f}\n" - ) - for obstacle in dynamic_obstacle_metrics + # Advance agent + agent.heading = (agent.heading + agent.yaw_rate) % (2 * math.pi) + agent_pos = agent_pos + np.array([ + math.cos(agent.heading) * agent.speed, + math.sin(agent.heading) * agent.speed, ]) + agent.position = tuple(agent_pos) + agent_circle.center = agent_pos + agent_tri.set_xy(_triangle_vertices(agent_pos, agent.heading)) + + # Advance obstacles + for i, obs in enumerate(obstacles): + obs.heading = (obs.heading + obs.yaw_rate) % (2 * math.pi) + obs_positions[i] = obs_positions[i] + np.array([ + math.cos(obs.heading) * obs.speed, + math.sin(obs.heading) * obs.speed, + ]) + obs.position = tuple(obs_positions[i]) + obs_circles[i].center = obs_positions[i] + obs_tris[i].set_xy(_triangle_vertices(obs_positions[i], obs.heading)) + + # Compute metrics + metrics = calculate_obstacle_metrics_for_agent(agent=agent, obstacles=obstacles) + I1 = calc_I1(agent=agent, dynamic_obstacles_with_metrics=metrics, dsf=dsf) + I2 = calc_I2(I1=I1, dynamic_obstacles_with_metrics=metrics, dsf=dsf) + I3 = calc_I3(dynamic_obstacles_with_metrics=metrics, dsf=dsf, time_of_interest=15) + uIoI = unionise_indices_of_interest(I1, I2, I3) - from datetime import datetime - # Update metrics text - timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S") - metrics_str = ( - "Scenario Metadata\n" - f"Distance Threshold (meters): {dsf}\n" - f"Timestamp: {timestamp}\n\n" - "Agent Metadata\n" - f"Tag: Agent Vessel\n" - f"Position: {agent.position}\n" - f"Orientation (yaw): {normalize_angle(quaternion_to_yaw(agent.orientation)):.2f}\n" - f"Velocity: {agent.velocity}\n" - f"yaw_rate: {agent.yaw_rate}\n\n" - "Obstacle Metadata\n" - f"{obstacle_metadata}\n\n" - "Indices Of Interest\n" - f"I1: {[obstacle.dynamic_obstacle.tag for obstacle in I1]}\n" - f"I2: {[obstacle.dynamic_obstacle.tag for obstacle in I2]}\n" - f"I3: {[obstacle.dynamic_obstacle.tag for obstacle in I3]}\n\n" - f"uIoI: {[obstacle.dynamic_obstacle.tag for obstacle in uIoI]}\n\n" + obs_meta = "\n".join([ + f"Tag: {m.obstacle.tag}\n" + f" TCPA: {m.tcpa:.2f} DCPA: {m.dcpa:.2f}" + for m in metrics + ]) + metrics_text.set_text( + f"Scenario: {scenario_name}\n" + f"DSF: {dsf} m {datetime.now().strftime('%H:%M:%S')}\n\n" + f"Agent\n" + f" pos: ({agent.position[0]:.1f}, {agent.position[1]:.1f})\n" + f" heading: {math.degrees(agent.heading):.1f}Β° speed: {agent.speed}\n\n" + f"Obstacles\n{obs_meta}\n\n" + f"I1: {[m.obstacle.tag for m in I1]}\n" + f"I2: {[m.obstacle.tag for m in I2]}\n" + f"I3: {[m.obstacle.tag for m in I3]}\n" + f"uIoI: {[m.obstacle.tag for m in uIoI]}" ) - metrics_text.set_text(metrics_str) - - # Create or update the unsafe set patch. - unsafe_set_vertices = create_unsafe_set(agent, obstacles, dsf) - if unsafe_set_vertices: - if not hasattr(update, 'unsafe_patch'): - update.unsafe_patch = patches.Polygon(unsafe_set_vertices, closed=True, edgecolor='black', - facecolor='#ff7f0e', alpha=0.5) - ax.add_patch(update.unsafe_patch) + # Update unsafe set patch + verts = create_unsafe_set(agent, obstacles, dsf) + if verts: + if unsafe_patch is None: + unsafe_patch = patches.Polygon( + verts, closed=True, edgecolor="black", facecolor="#ff7f0e", alpha=0.5 + ) + ax.add_patch(unsafe_patch) else: - update.unsafe_patch.set_xy(unsafe_set_vertices) - else: - if hasattr(update, 'unsafe_patch'): - update.unsafe_patch.set_visible(False) - - artists = [agent_circle, agent_triangle] + obstacle_circles + obstacle_triangles - if hasattr(update, 'unsafe_patch') and update.unsafe_patch.get_visible(): - artists.append(update.unsafe_patch) - artists.append(metrics_text) + unsafe_patch.set_xy(verts) + unsafe_patch.set_visible(True) + elif unsafe_patch is not None: + unsafe_patch.set_visible(False) + + artists = [agent_circle, agent_tri, *obs_circles, *obs_tris, metrics_text] + if unsafe_patch is not None and unsafe_patch.get_visible(): + artists.append(unsafe_patch) return artists if animate: - from matplotlib.animation import FuncAnimation - ani = FuncAnimation(fig, update, frames=range(100), interval=100, blit=True) - plt.suptitle(f"Animation for {scenario_name}") + ani = FuncAnimation(fig, update, frames=100, interval=100, blit=True) + plt.suptitle(f"Risk Envelope β€” {scenario_name}") gif_path = PLOT_DIR / f"{scenario_name.replace(':', '_')}_animation.gif" - ani.save(gif_path, writer='pillow', fps=10) - plt.close() + ani.save(gif_path, writer="pillow", fps=10) else: - plt.suptitle(f"{scenario_name}") - plt.xlabel("X-Axis (meters)") - plt.ylabel("Y-Axis (meters)") - plt.savefig(PLOT_DIR / f"{scenario_name.replace(':', '_')}_plot.png", facecolor=fig.get_facecolor()) - plt.close() + update(0) + plt.suptitle(f"Risk Envelope β€” {scenario_name}") + plt.savefig( + PLOT_DIR / f"{scenario_name.replace(':', '_')}_plot.png", + facecolor=fig.get_facecolor(), + ) + plt.close() -@pytest.mark.parametrize("scenario_file", get_scenario_files()) -def test_unsafe_set_gen(scenario_file): - """Test unsafe set generation and visualization for given scenarios.""" - scenario = load_yaml(scenario_file) - width, height = int(scenario['matrix']['width']), int(scenario['matrix']['height']) - - agent = Agent(**scenario['agent_vessel']) - if scenario.get('obstacles') is not None: - obstacles = [DynamicObstacle(**obs) for obs in scenario.get('obstacles', [])] - else: - obstacles = [] +# --------------------------------------------------------------------------- +# Pytest parametrised test +# --------------------------------------------------------------------------- - plot_scenario(agent, obstacles, width, height, scenario_file.stem, scenario.get('dsf'), animate=True) - -def main(): +@pytest.mark.parametrize("scenario_file", get_scenario_files()) +def test_risk_envelope_scenario(scenario_file: Path) -> None: + """Load a scenario, run create_unsafe_set, and produce a visualisation.""" + scenario = _load_yaml(scenario_file) + width = float(scenario["matrix"]["width"]) + height = float(scenario["matrix"]["height"]) + dsf = float(scenario.get("dsf", 10.0)) + + agent = _agent_from_dict(scenario["agent_vessel"]) + obstacles = ( + [_obstacle_from_dict(o) for o in scenario["obstacles"]] + if scenario.get("obstacles") + else [] + ) + + # Smoke-test: create_unsafe_set must return a list (possibly empty) + result = create_unsafe_set(agent=agent, obstacles=obstacles, dsf=dsf) + assert isinstance(result, list) + for vertex in result: + assert len(vertex) == 2 + + plot_scenario(agent, obstacles, width, height, scenario_file.stem, dsf, animate=True) + + +# --------------------------------------------------------------------------- +# Entry point for direct execution +# --------------------------------------------------------------------------- + +def main() -> None: for scenario_file in get_scenario_files(): - test_unsafe_set_gen(scenario_file) + test_risk_envelope_scenario(scenario_file) + -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tests/unit_tests/indices_of_interest/test_calculate_I2.py b/tests/unit_tests/indices_of_interest/test_calculate_I2.py index 6f9b0c3..990f32c 100644 --- a/tests/unit_tests/indices_of_interest/test_calculate_I2.py +++ b/tests/unit_tests/indices_of_interest/test_calculate_I2.py @@ -1,179 +1,48 @@ -# import pytest -# from unittest.mock import MagicMock -# import sys -# import os - -# # Make sure the module path is correctly added -# # sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../../src/colav_unsafe_set'))) -# from colav_unsafe_set.objects import Agent, DynamicObstacleWithMetrics, DynamicObstacle -# from src.colav_unsafe_set.indices_of_interest import calc_I2 - - -# @pytest.fixture -# def agent_vessel(): -# # Setup basic mock data for agent vessel -# agent_vessel = Agent( -# position=(float(0), float(0), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) -# return agent_vessel - - -# def test_calc_I2_empty_I1(agent_vessel): -# # Test when I1 is empty -# I1 = [] -# dynamic_obstacle = DynamicObstacle( -# tag= "mock_obstacle", -# position= (float(10), float(10), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate I2 -# I2 = calc_I2(I1, dynamic_obstacles, dsf) - -# # Assert that I2 is empty since I1 is empty -# assert len(I2) == 0 - - -# def test_calc_I2_no_matching_obstacles(agent_vessel): -# # Test when there are no matching obstacles in I1 and dynamic_obstacles -# dynamic_obstacle1 = DynamicObstacle( -# tag= "mock_obstacle1", -# position= (float(3), float(3), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# dynamic_obstacle2 = DynamicObstacle( -# tag= "mock_obstacle2", -# position= (float(10), float(10), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# # Wrap obstacles in DynamicObstacleWithMetrics -# dynamic_obstacle_with_metrics1 = DynamicObstacleWithMetrics(dynamic_obstacle1, dcpa=1.0, tcpa=2.0) -# dynamic_obstacle_with_metrics2 = DynamicObstacleWithMetrics(dynamic_obstacle2, dcpa=1.0, tcpa=2.0) - -# # I1 contains one obstacle that doesn't match any in dynamic_obstacles -# I1 = [dynamic_obstacle_with_metrics1] - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics2] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate I2 -# I2 = calc_I2(I1, dynamic_obstacles, dsf) - -# # Assert that I2 is empty since there are no matching obstacles -# assert len(I2) == 0 - - -# def test_calc_I2_with_matching_obstacles(agent_vessel): -# # Test when I1 contains obstacles that match with dynamic_obstacles -# dynamic_obstacle1 = DynamicObstacle( -# tag= "mock_obstacle1", -# position= (float(3), float(3), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# dynamic_obstacle2 = DynamicObstacle( -# tag= "mock_obstacle2", -# position= (float(10), float(10), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# # Wrap obstacles in DynamicObstacleWithMetrics -# dynamic_obstacle_with_metrics1 = DynamicObstacleWithMetrics(dynamic_obstacle1, dcpa=1.0, tcpa=2.0) -# dynamic_obstacle_with_metrics2 = DynamicObstacleWithMetrics(dynamic_obstacle2, dcpa=1.0, tcpa=2.0) - -# # I1 contains one obstacle that matches with dynamic_obstacles -# I1 = [dynamic_obstacle_with_metrics1] - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics1, dynamic_obstacle_with_metrics2] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate I2 -# I2 = calc_I2(I1, dynamic_obstacles, dsf) - -# # Assert that the matching obstacle is in I2 -# assert len(I2) == 0 - -# def test_calc_I2_multiple_obstacles(agent_vessel): -# # Test with multiple obstacles in I1 and dynamic_obstacles -# dynamic_obstacle1 = DynamicObstacle( -# tag= "mock_obstacle1", -# position= (float(3), float(3), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# dynamic_obstacle2 = DynamicObstacle( -# tag= "mock_obstacle2", -# position= (float(5), float(5), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) -# dynamic_obstacle3 = DynamicObstacle( -# tag= "mock_obstacle3", -# position= (float(8), float(8), float(0)), -# orientation=(float(0), float(0), float(0), float(0)), -# velocity=float(10), -# yaw_rate=float(0), -# safety_radius=float(1) -# ) - -# # Wrap obstacles in DynamicObstacleWithMetrics -# dynamic_obstacle_with_metrics1 = DynamicObstacleWithMetrics(dynamic_obstacle1, dcpa=1.0, tcpa=2.0) -# dynamic_obstacle_with_metrics2 = DynamicObstacleWithMetrics(dynamic_obstacle2, dcpa=1.0, tcpa=2.0) -# dynamic_obstacle_with_metrics3 = DynamicObstacleWithMetrics(dynamic_obstacle3, dcpa=1.0, tcpa=2.0) - -# # I1 contains two obstacles -# I1 = [dynamic_obstacle_with_metrics1, dynamic_obstacle_with_metrics2] - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics1, dynamic_obstacle_with_metrics2, dynamic_obstacle_with_metrics3] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate I2 -# I2 = calc_I2(I1, dynamic_obstacles, dsf) - -# # Assert that only the obstacles that are within the distance safety threshold are in I2 -# assert len(I2) == 2 - - -# def main(): -# pytest.main() - - -# if __name__ == '__main__': -# main() +"""Unit tests for calc_I2 β€” inter-obstacle proximity filter.""" +import pytest +from riskenv import calc_I2 +from tests.conftest import make_obs_with_metrics + + +class TestCalcI2: + def test_empty_I1_returns_empty(self): + other = make_obs_with_metrics(position=(3.0, 0.0), dcpa=1.0, tcpa=5.0) + result = calc_I2(I1=[], dynamic_obstacles_with_metrics=[other], dsf=5.0) + assert result == [] + + def test_obstacle_with_nearby_other_is_included(self): + # obs1 at (0,0), obs2 at (4,0): adjusted distance = 4-2=2 ≀ dsf=5 + obs1 = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs1") + obs2 = make_obs_with_metrics(position=(4.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs2") + result = calc_I2(I1=[obs1], dynamic_obstacles_with_metrics=[obs1, obs2], dsf=5.0) + assert obs1 in result + + def test_obstacle_with_no_nearby_other_is_excluded(self): + # obs1 at (0,0), obs2 at (50,0): adjusted distance=48 > dsf=5 + obs1 = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs1") + obs2 = make_obs_with_metrics(position=(50.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs2") + result = calc_I2(I1=[obs1], dynamic_obstacles_with_metrics=[obs1, obs2], dsf=5.0) + assert obs1 not in result + + def test_obstacle_is_not_compared_to_itself(self): + # Only one obstacle total β€” cannot be "near" itself + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=5.0) + result = calc_I2(I1=[obs], dynamic_obstacles_with_metrics=[obs], dsf=5.0) + assert result == [] + + def test_multiple_obstacles_only_near_ones_included(self): + obs1 = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs1") + obs2 = make_obs_with_metrics(position=(4.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs2") # near obs1 + obs3 = make_obs_with_metrics(position=(50.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs3") # far from all + all_obs = [obs1, obs2, obs3] + result = calc_I2(I1=[obs1, obs3], dynamic_obstacles_with_metrics=all_obs, dsf=5.0) + assert obs1 in result + assert obs3 not in result + + def test_each_I1_obstacle_added_at_most_once(self): + """An I1 obstacle with multiple nearby others should appear once in I2.""" + obs1 = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs1") + obs2 = make_obs_with_metrics(position=(2.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs2") + obs3 = make_obs_with_metrics(position=(3.0, 0.0), dcpa=1.0, tcpa=5.0, tag="obs3") + result = calc_I2(I1=[obs1], dynamic_obstacles_with_metrics=[obs1, obs2, obs3], dsf=5.0) + assert result.count(obs1) == 1 diff --git a/tests/unit_tests/indices_of_interest/test_calculate_I3.py b/tests/unit_tests/indices_of_interest/test_calculate_I3.py index 6ba4cf6..2be39df 100644 --- a/tests/unit_tests/indices_of_interest/test_calculate_I3.py +++ b/tests/unit_tests/indices_of_interest/test_calculate_I3.py @@ -1,127 +1,58 @@ -# import pytest -# from unittest.mock import MagicMock -# from colav_unsafe_set.objects import Agent, DynamicObstacleWithMetrics, DynamicObstacle -# from src.colav_unsafe_set.indices_of_interest import calc_I3 - -# def test_calc_I3_outside_safety_threshold(): -# # Create a mock dynamic obstacle that is outside the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag = "mock_obstacle", -# position = [10.0, 10.0, 0.0], -# orientation=[0.0, 0.0, 0.0, 0.0], -# velocity=10.0, -# yaw_rate=0.0, -# safety_radius=1.0 -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=6.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I3 = calc_I3(dynamic_obstacles, dsf) - -# # Assert that the obstacle is not within the safety threshold -# assert len(I3) == 0 - -# def test_calc_I3_within_safety_threshold(): -# # Create a mock dynamic obstacle that is within the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag = "mock_obstacle", -# position = [3.0, 3.0, 0.0], -# orientation=[0.0, 0.0, 0.0, 0.0], -# velocity=10.0, -# yaw_rate=0.0, -# safety_radius=1.0 -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=4.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I3 = calc_I3(dynamic_obstacles, dsf) - -# # Assert that the obstacle is within the safety threshold -# assert len(I3) == 1 - -# def test_calc_I3_at_safety_threshold(): -# # Create a mock dynamic obstacle that is exactly at the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag = "mock_obstacle", -# position = [5.0, 5.0, 0.0], -# orientation=[0.0, 0.0, 0.0, 0.0], -# velocity=10.0, -# yaw_rate=0.0, -# safety_radius=1.0 -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=5.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I3 = calc_I3(dynamic_obstacles, dsf) - -# # Assert that the obstacle is within the safety threshold -# assert len(I3) == 1 - -# def test_calc_I3_multiple_obstacles(): -# # Create multiple mock dynamic obstacles -# dynamic_obstacle1 = DynamicObstacle( -# tag = "mock_obstacle", -# position = [3.0, 3.0, 0.0], -# orientation=[0.0, 0.0, 0.0, 0.0], -# velocity=10.0, -# yaw_rate=0.0, -# safety_radius=1.0 -# ) -# dynamic_obstacle2 = DynamicObstacle( -# tag = "mock_obstacle", -# position = [10.0, 10.0, 0.0], -# orientation=[0.0, 0.0, 0.0, 0.0], -# velocity=10.0, -# yaw_rate=0.0, -# safety_radius=1.0 -# ) - -# # Wrap obstacles in DynamicObstacleWithMetrics -# dynamic_obstacle_with_metrics1 = DynamicObstacleWithMetrics(dynamic_obstacle1, dcpa=4.0, tcpa=2.0) -# dynamic_obstacle_with_metrics2 = DynamicObstacleWithMetrics(dynamic_obstacle2, dcpa=6.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics1, dynamic_obstacle_with_metrics2] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I3 = calc_I3(dynamic_obstacles, dsf) - -# # Assert that only the first obstacle is within the safety threshold -# assert len(I3) == 1 - -# def test_calc_I3_no_obstacles(): -# # Test when no obstacles are provided -# dynamic_obstacles = [] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I3 = calc_I3(dynamic_obstacles, dsf) - -# # Assert that the unsafe set is empty -# assert len(I3) == 0 +"""Unit tests for calc_I3 β€” CPA-based risk filter (DCPA ≀ dsf and TCPA ≀ time_of_interest).""" +import math +import pytest +from riskenv import calc_I3 +from tests.conftest import make_obs_with_metrics + + +DSF = 5.0 +TOI = 15.0 # time_of_interest in seconds + + +class TestCalcI3: + def test_obstacle_within_dcpa_and_tcpa_is_included(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=10.0) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs in result + + def test_obstacle_dcpa_exactly_at_dsf_is_included(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=DSF, tcpa=10.0) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs in result + + def test_obstacle_tcpa_exactly_at_time_of_interest_is_included(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=TOI) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs in result + + def test_obstacle_dcpa_exceeds_dsf_is_excluded(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=DSF + 0.1, tcpa=10.0) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs not in result + + def test_obstacle_tcpa_exceeds_time_of_interest_is_excluded(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=TOI + 1.0) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs not in result + + def test_nan_dcpa_is_excluded(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=float('nan'), tcpa=10.0) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs not in result + + def test_nan_tcpa_is_excluded(self): + obs = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=float('nan')) + result = calc_I3([obs], dsf=DSF, time_of_interest=TOI) + assert obs not in result + + def test_empty_obstacle_list_returns_empty(self): + assert calc_I3([], dsf=DSF, time_of_interest=TOI) == [] + + def test_multiple_obstacles_mixed_inclusion(self): + inside = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=10.0, tag="in") + bad_d = make_obs_with_metrics(position=(0.0, 0.0), dcpa=6.0, tcpa=10.0, tag="bad_d") + bad_t = make_obs_with_metrics(position=(0.0, 0.0), dcpa=4.0, tcpa=20.0, tag="bad_t") + result = calc_I3([inside, bad_d, bad_t], dsf=DSF, time_of_interest=TOI) + assert inside in result + assert bad_d not in result + assert bad_t not in result diff --git a/tests/unit_tests/indices_of_interest/test_calculate_i1.py b/tests/unit_tests/indices_of_interest/test_calculate_i1.py index 6abb497..8b321a8 100644 --- a/tests/unit_tests/indices_of_interest/test_calculate_i1.py +++ b/tests/unit_tests/indices_of_interest/test_calculate_i1.py @@ -1,183 +1,50 @@ -# import pytest -# from unittest.mock import MagicMock -# import sys -# import os - -# # Make sure the module path is correctly added -# # sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../../src/colav_unsafe_set'))) -# from colav_unsafe_set.objects import Agent, DynamicObstacleWithMetrics, DynamicObstacle -# from src.colav_unsafe_set.indices_of_interest import calc_I1 - - -# @pytest.fixture -# def agent_vessel(): -# # Setup basic mock data for agent vessel -# agent_vessel = Agent( -# position=[float(0), float(0), float(0)], -# orientation=[float(0), float(0), float(0), float(0.0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1) -# ) -# return agent_vessel - -# def test_calc_I1_outside_safety_threshold(agent_vessel): -# # Create a mock dynamic obstacle that is outside the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag='mock_obstacle', -# position=[float(10), float(10), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the obstacle is not within the unsafe set -# assert len(I1) == 0 - - -# def test_calc_I1_within_safety_threshold(agent_vessel): -# # Create a mock dynamic obstacle that is within the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag='mock_obstacle', -# position=[float(3), float(3), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the obstacle is within the unsafe set -# assert len(I1) == 1 - - -# def test_calc_I1_at_safety_threshold(agent_vessel): -# # Create a mock dynamic obstacle that is exactly at the safety threshold -# dynamic_obstacle = DynamicObstacle( -# tag='mock_obstacle', -# position=[float(5), float(5), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.1 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the obstacle is within the unsafe set -# assert len(I1) == 1 - - -# def test_calc_I1_multiple_obstacles(agent_vessel): -# # Create multiple mock dynamic obstacles -# dynamic_obstacle1 = DynamicObstacle( -# tag='mock_obstacle1', -# position=[float(3), float(3), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) -# dynamic_obstacle2 = DynamicObstacle( -# tag='mock_obstacle2', -# position=[float(10), float(10), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) - -# # Wrap obstacles in DynamicObstacleWithMetrics -# dynamic_obstacle_with_metrics1 = DynamicObstacleWithMetrics(dynamic_obstacle1, dcpa=1.0, tcpa=2.0) -# dynamic_obstacle_with_metrics2 = DynamicObstacleWithMetrics(dynamic_obstacle2, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics1, dynamic_obstacle_with_metrics2] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that only the first obstacle is within the unsafe set -# assert len(I1) == 1 - - -# def test_calc_I1_no_obstacles(agent_vessel): -# # Test when no obstacles are provided -# dynamic_obstacles = [] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the unsafe set is empty -# assert len(I1) == 0 - - -# def test_calc_I1_zero_safety_radius(agent_vessel): -# # Modify the agent's safety radius to 0 -# agent_vessel.safety_radius = 0.0 -# dynamic_obstacle = DynamicObstacle( -# tag='mock_obstacle', -# position=[float(3), float(3), float(0)], -# orientation=[float(0), float(0), float(0), float(0)], -# yaw_rate=float(0.0), -# velocity=float(10), -# safety_radius=float(1.0) -# ) - -# dynamic_obstacle_with_metrics = DynamicObstacleWithMetrics(dynamic_obstacle, dcpa=1.0, tcpa=2.0) - -# # List of obstacles for the test -# dynamic_obstacles = [dynamic_obstacle_with_metrics] - -# # Define the safety threshold -# dsf = 5.0 - -# # Call the function to calculate the unsafe set -# I1 = calc_I1(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the obstacle is within the unsafe set (since agent has no safety buffer) -# assert len(I1) == 1 - -# def main(): -# pytest.main() - - -# if __name__ == '__main__': -# main() +"""Unit tests for calc_I1 β€” proximity filter (agent ↔ obstacle distance).""" +import pytest +from riskenv import Agent, calc_I1 +from tests.conftest import make_obs_with_metrics + + +@pytest.fixture +def agent(): + return Agent(position=(0.0, 0.0), heading=0.0, speed=10.0, + yaw_rate=0.0, safety_radius=1.0) + + +class TestCalcI1: + # adjusted distance = euclidean - (agent_radius + obs_radius) + + def test_obstacle_within_dsf_is_included(self, agent): + # distance=3, adjusted=3-2=1 ≀ dsf=5 + obs = make_obs_with_metrics(position=(3.0, 0.0), dcpa=1.0, tcpa=5.0) + result = calc_I1(agent, [obs], dsf=5.0) + assert obs in result + + def test_obstacle_outside_dsf_is_excluded(self, agent): + # distance=50, adjusted=48 > dsf=5 + obs = make_obs_with_metrics(position=(50.0, 0.0), dcpa=1.0, tcpa=5.0) + result = calc_I1(agent, [obs], dsf=5.0) + assert obs not in result + + def test_obstacle_exactly_at_dsf_boundary_is_included(self, agent): + # obstacle at (7,0): distance=7, adjusted=7-2=5 == dsf=5 + obs = make_obs_with_metrics(position=(7.0, 0.0), dcpa=1.0, tcpa=5.0) + result = calc_I1(agent, [obs], dsf=5.0) + assert obs in result + + def test_empty_obstacle_list_returns_empty(self, agent): + assert calc_I1(agent, [], dsf=5.0) == [] + + def test_only_nearby_obstacles_are_returned(self, agent): + near = make_obs_with_metrics(position=(3.0, 0.0), dcpa=1.0, tcpa=5.0, tag="near") + far = make_obs_with_metrics(position=(50.0, 0.0), dcpa=1.0, tcpa=5.0, tag="far") + result = calc_I1(agent, [near, far], dsf=5.0) + assert near in result + assert far not in result + + def test_2d_and_3d_positions_are_treated_consistently(self): + # Agent with 3-D position β€” xy-projection should still work + agent_3d = Agent(position=(0.0, 0.0, 10.0), heading=0.0, speed=10.0, + yaw_rate=0.0, safety_radius=1.0) + obs = make_obs_with_metrics(position=(3.0, 0.0, 99.0), dcpa=1.0, tcpa=5.0) + result = calc_I1(agent_3d, [obs], dsf=5.0) + assert obs in result diff --git a/tests/unit_tests/indices_of_interest/test_indices_of_interest.py b/tests/unit_tests/indices_of_interest/test_indices_of_interest.py index 89688aa..acba211 100644 --- a/tests/unit_tests/indices_of_interest/test_indices_of_interest.py +++ b/tests/unit_tests/indices_of_interest/test_indices_of_interest.py @@ -1,191 +1,3 @@ -# import pytest -# from colav_unsafe_set.indices_of_interest import calc_I1, calc_I2, calc_I3 -# from colav_unsafe_set.objects import ( -# DynamicObstacle, -# DynamicOB, -# DynamicObject, -# Configuration, -# ) - - -# # obstacles = [ -# # # Test 1: No velocity, same position -# # ( -# # DynamicObstacle( -# # id = 'obstacle_1', -# # DYnamicOBstacleWith = DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=0.0), -# # ), -# # yaw_rate=0.0, -# # velocity=0.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=0.0), -# # ), -# # yaw_rate=0.0, -# # velocity=0.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 0.0, # expected dcpa -# # 0.0, # expected tcpa -# # ), -# # # Test 2: Same velocity, different direction -# # ( -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=100.0, y=100.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 141.4213562373095, # expected dcpa -# # 14.14213562373095, # expected tcpa -# # ), -# # # Test 3: Different velocities, intersecting paths -# # ( -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=100.0, y=100.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=15.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 141.4213562373095, # expected dcpa -# # 9.20753323377405, # expected tcpa -# # ), -# # # Test 4: Different velocities, no interception (moving in parallel) -# # ( -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=100.0, y=200.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 100.0, # expected dcpa -# # float("inf"), # expected tcpa (no interception) -# # ), -# # # Test 5: High-speed approach, small time to collision -# # ( -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=50.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=1000.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=50.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 1000.0, # expected dcpa -# # 20.0, # expected tcpa -# # ), -# # # Test 6: Moving perpendicular to each other (no interception) -# # ( -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=0.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # DynamicObject( -# # configuration=DynamicObject.Configuration( -# # pose=DynamicObject.Pose( -# # position=DynamicObject.Position(x=100.0, y=0.0, z=0.0), -# # orientation=DynamicObject.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# # ), -# # yaw_rate=0.0, -# # velocity=10.0, -# # ), -# # safety_radius=5.0, -# # ), -# # 100.0, # expected dcpa -# # float("inf"), # expected tcpa (no interception) -# # ), -# # ] - - -# # @pytest.mark.parametrize( -# # "input_value, expected_output", -# # [ -# # (10, 100), # Example test case for calc_I1 -# # (5, 25), # Example test case for calc_I2 -# # (2, 4) # Example test case for calc_I3 -# # ] -# # ) -# # def test_calc_I1(input_value, expected_output): -# # def test_calc_I1(): -# # # You should replace calc_I1 with your actual function and its logic -# # # result = calc_I1(input_value) # Assume calc_I1 processes input_value -# # assert 0 == 0 +# Placeholder β€” individual index-of-interest functions are tested in their +# dedicated files: test_calculate_i1.py, test_calculate_I2.py, test_calculate_I3.py, +# and test_unionise_indices_of_interest.py. diff --git a/tests/unit_tests/indices_of_interest/test_unionise_indices_of_interest.py b/tests/unit_tests/indices_of_interest/test_unionise_indices_of_interest.py index 1ba52d9..5b37c6d 100644 --- a/tests/unit_tests/indices_of_interest/test_unionise_indices_of_interest.py +++ b/tests/unit_tests/indices_of_interest/test_unionise_indices_of_interest.py @@ -1,97 +1,54 @@ -# import pytest -# from colav_unsafe_set.objects import DynamicObstacleWithMetrics -# from typing import List -# from colav_unsafe_set.indices_of_interest import unionise_indices_of_interest +"""Unit tests for unionise_indices_of_interest β€” deduplication by object identity.""" +import pytest +from riskenv import unionise_indices_of_interest +from tests.conftest import make_obs_with_metrics -# @pytest.fixture -# def obstacle1(): -# # Create mock dynamic obstacle with metrics -# return DynamicObstacleWithMetrics(None, dcpa=1.0, tcpa=2.0) +@pytest.fixture +def obs1(): + return make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=2.0, tag="obs1") -# @pytest.fixture -# def obstacle2(): -# # Create another mock dynamic obstacle with metrics -# return DynamicObstacleWithMetrics(None, dcpa=2.0, tcpa=3.0) +@pytest.fixture +def obs2(): + return make_obs_with_metrics(position=(5.0, 0.0), dcpa=2.0, tcpa=3.0, tag="obs2") -# @pytest.fixture -# def obstacle3(): -# # Create another mock dynamic obstacle with metrics -# return DynamicObstacleWithMetrics(None, dcpa=3.0, tcpa=4.0) +@pytest.fixture +def obs3(): + return make_obs_with_metrics(position=(10.0, 0.0), dcpa=3.0, tcpa=4.0, tag="obs3") -# def test_unionise_indices_of_interest_empty_lists(): -# # Test the union function with empty lists -# I1 = [] -# I2 = [] -# I3 = [] - -# result = unionise_indices_of_interest(I1, I2, I3) - -# # Assert that the result is an empty list -# assert result == [] +class TestUnioniseIndicesOfInterest: + def test_all_empty_returns_empty(self): + assert unionise_indices_of_interest([], [], []) == [] -# def test_unionise_indices_of_interest_no_common_elements(obstacle1, obstacle2, obstacle3): -# # Test the union function with no overlapping elements -# I1 = [obstacle1] -# I2 = [obstacle2] -# I3 = [obstacle3] - -# result = unionise_indices_of_interest(I1, I2, I3) - -# # Assert that all obstacles are in the result -# assert len(result) == 3 -# assert obstacle1 in result -# assert obstacle2 in result -# assert obstacle3 in result + def test_no_duplicates_all_elements_returned(self, obs1, obs2, obs3): + result = unionise_indices_of_interest([obs1], [obs2], [obs3]) + assert len(result) == 3 + assert obs1 in result + assert obs2 in result + assert obs3 in result + def test_duplicates_across_sets_are_deduplicated(self, obs1, obs2): + # obs1 appears in I1 and I3, obs2 appears in I2 and I3 + result = unionise_indices_of_interest([obs1], [obs2], [obs1, obs2]) + assert len(result) == 2 + assert obs1 in result + assert obs2 in result -# def test_unionise_indices_of_interest_with_common_elements(obstacle1, obstacle2): -# # Test the union function with some overlapping elements -# I1 = [obstacle1, obstacle2] -# I2 = [obstacle2] -# I3 = [obstacle1] - -# result = unionise_indices_of_interest(I1, I2, I3) - -# # Assert that duplicates are removed and only unique obstacles remain -# assert len(result) == 2 -# assert obstacle1 in result -# assert obstacle2 in result + def test_same_obstacle_in_all_sets_appears_once(self, obs1): + result = unionise_indices_of_interest([obs1], [obs1], [obs1]) + assert len(result) == 1 + assert result[0] is obs1 + def test_deduplication_is_by_identity_not_equality(self): + # Two distinct objects with identical field values must both be kept + a = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=2.0, tag="same") + b = make_obs_with_metrics(position=(0.0, 0.0), dcpa=1.0, tcpa=2.0, tag="same") + result = unionise_indices_of_interest([a], [b], []) + assert len(result) == 2 -# def test_unionise_indices_of_interest_all_identical(obstacle1): -# # Test the union function where all lists are identical -# I1 = [obstacle1] -# I2 = [obstacle1] -# I3 = [obstacle1] - -# result = unionise_indices_of_interest(I1, I2, I3) - -# # Assert that only one obstacle is in the result (since duplicates are removed) -# assert len(result) == 1 -# assert obstacle1 in result - - -# def test_unionise_indices_of_interest_different_types(obstacle1, obstacle2): -# # Test the union function with different obstacles in each list -# I1 = [obstacle1] -# I2 = [obstacle2] -# I3 = [] - -# result = unionise_indices_of_interest(I1, I2, I3) - -# # Assert that both obstacles are in the result -# assert len(result) == 2 -# assert obstacle1 in result -# assert obstacle2 in result - - -# def main(): -# pytest.main() - - -# if __name__ == '__main__': -# main() + def test_single_non_empty_set(self, obs1, obs2): + result = unionise_indices_of_interest([obs1, obs2], [], []) + assert len(result) == 2 diff --git a/tests/unit_tests/risk_assesment/__init__.py b/tests/unit_tests/position_prediction/__init__.py similarity index 100% rename from tests/unit_tests/risk_assesment/__init__.py rename to tests/unit_tests/position_prediction/__init__.py diff --git a/tests/unit_tests/position_prediction/test_position_prediction.py b/tests/unit_tests/position_prediction/test_position_prediction.py new file mode 100644 index 0000000..bb5eac5 --- /dev/null +++ b/tests/unit_tests/position_prediction/test_position_prediction.py @@ -0,0 +1,59 @@ +"""Unit tests for riskenv.position_prediction β€” predict_position.""" +import math +import numpy as np +import pytest +from pytest import approx +from riskenv import predict_position + + +class TestPredictPosition: + def test_straight_line_motion_2d(self): + result = predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=5.0) + assert result[0] == approx(50.0, abs=1e-9) + assert result[1] == approx(0.0, abs=1e-9) + + def test_z_coordinate_unchanged_for_3d_input(self): + result = predict_position((0.0, 0.0, 5.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=5.0) + assert result[2] == approx(5.0, abs=1e-9) + + def test_output_length_matches_input_length_2d(self): + result = predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=1.0) + assert len(result) == 2 + + def test_output_length_matches_input_length_3d(self): + result = predict_position((0.0, 0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=1.0) + assert len(result) == 3 + + def test_zero_speed_stays_in_place(self): + result = predict_position((3.0, 7.0), heading=1.0, speed=0.0, yaw_rate=0.0, dt=5.0) + assert result[0] == approx(3.0, abs=1e-9) + assert result[1] == approx(7.0, abs=1e-9) + + def test_yaw_rate_changes_heading_before_displacement(self): + # heading=0, yaw_rate=Ο€/2, dt=1 β†’ yaw_new=Ο€/2 β†’ moves north + result = predict_position((0.0, 0.0), heading=0.0, speed=10.0, + yaw_rate=math.pi / 2, dt=1.0) + assert result[0] == approx(0.0, abs=1e-9) + assert result[1] == approx(10.0, abs=1e-9) + + def test_heading_north_moves_in_y_direction(self): + result = predict_position((0.0, 0.0), heading=math.pi / 2, + speed=10.0, yaw_rate=0.0, dt=2.0) + assert result[0] == approx(0.0, abs=1e-9) + assert result[1] == approx(20.0, abs=1e-9) + + def test_negative_dt_raises_value_error(self): + with pytest.raises(ValueError): + predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=-1.0) + + def test_zero_dt_raises_value_error(self): + with pytest.raises(ValueError): + predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=0.0) + + def test_nan_dt_raises_value_error(self): + with pytest.raises(ValueError): + predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=float('nan')) + + def test_returns_numpy_array(self): + result = predict_position((0.0, 0.0), heading=0.0, speed=10.0, yaw_rate=0.0, dt=1.0) + assert isinstance(result, np.ndarray) diff --git a/tests/unit_tests/risk_assesment/test_risk_assesment.py b/tests/unit_tests/risk_assesment/test_risk_assesment.py deleted file mode 100644 index b43355f..0000000 --- a/tests/unit_tests/risk_assesment/test_risk_assesment.py +++ /dev/null @@ -1,212 +0,0 @@ -# from src.colav_unsafe_set.risk_assessment import calc_cpa -# from src.colav_unsafe_set.objects import DynamicObject, DynamicObstacle, Configuration -# import pytest -# from pytest import approx - -# test_ids = [ -# "No velocity, same position", -# "Same velocity, different direction", -# "Different velocities, intersecting paths", -# "Different velocities, no interception (moving in parallel)", -# "High-speed approach, small time to collision", -# "Moving perpendicular to each other (no interception)", -# ] -# testdata = [ -# # Test 1: No velocity, same position -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=0.0), -# ), -# yaw_rate=0.0, -# velocity=0.0, -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=0.0), -# ), -# yaw_rate=0.0, -# velocity=0.0, -# ), -# safety_radius=5.0, -# ), -# float('nan'), # expected dcpa -# float('inf'), # expected tcpa -# ), -# # Test 2: Same velocity, different direction -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=100.0, y=100.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# 141.4213562373095, # expected dcpa -# 14.14213562373095, # expected tcpa -# ), -# # Test 3: Different velocities, intersecting paths -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, # Object 1 moving at 10.0 -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=100.0, y=100.0, z=0.0), # Starting point moved -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=-0.707, w=0.707), # Object 2 now moves towards Object 1 -# ), -# yaw_rate=0.0, -# velocity=10.0, # Object 2 moving at 10.0 -# ), -# safety_radius=5.0, -# ), -# 0.021361075105734158, # expected DCPA (they will intersect) -# 10.001510684305536, # expected TCPA (they will intersect in 14.14 seconds) -# ), -# # Test 4: Different velocities, no interception -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=100.0, y=200.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# 223.60679774997897, # expected dcpa -# 22.360679774997898, # expected tcpa (no interception) -# ), -# # Test 5: High-speed approach, small time to collision -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=50.0, -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=1000.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=50.0, -# ), -# safety_radius=5.0, -# ), -# 1000.0, # expected dcpa -# 20.0, # expected tcpa -# ), -# # Test 6: Moving perpendicular to each other (no interception) -# ( -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=0.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# DynamicObject( -# configuration=Configuration( -# pose=Configuration.Pose( -# position=Configuration.Pose.Position(x=100.0, y=0.0, z=0.0), -# orientation=Configuration.Pose.Orientation(x=0.0, y=0.0, z=0.0, w=1.0), -# ), -# yaw_rate=0.0, -# velocity=10.0, -# ), -# safety_radius=5.0, -# ), -# 100.0, # expected dcpa -# 10.0, # expected tcpa (no interception) -# ), -# ] -# @pytest.mark.parametrize( -# "agent_object, target_object, expected_dcpa, expected_tcpa", testdata, ids=test_ids -# ) -# def test_calc_dcpa_and_tcpa( -# agent_object: DynamicObject, -# target_object: DynamicObject, -# expected_dcpa: float, -# expected_tcpa: float, -# ): -# dcpa, tcpa = calc_cpa(agent_object, target_object) - -# print(f"Actual DCPA: {dcpa}, Expected DCPA: {expected_dcpa}") -# print(f"Actual TCPA: {tcpa}, Expected TCPA: {expected_tcpa}") -# import math - -# if math.isnan(expected_dcpa): -# assert math.isnan(dcpa) -# elif math.isinf(expected_dcpa): -# assert math.isinf(dcpa) -# else: -# assert float(dcpa) == approx(float(expected_dcpa), rel=1e-9) - -# if math.isnan(expected_tcpa): -# assert math.isnan(tcpa) -# elif math.isinf(expected_tcpa): -# assert math.isnan(dcpa) -# else: -# assert float(tcpa) == approx(float(expected_tcpa), rel=1e-9) - - -# def main(): -# test_calc_dcpa_and_tcpa() - -# if __name__ == '__main__': -# main() \ No newline at end of file diff --git a/tests/unit_tests/risk_assessment/__init__.py b/tests/unit_tests/risk_assessment/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/unit_tests/risk_assessment/test_risk_assessment.py b/tests/unit_tests/risk_assessment/test_risk_assessment.py new file mode 100644 index 0000000..8bad5f1 --- /dev/null +++ b/tests/unit_tests/risk_assessment/test_risk_assessment.py @@ -0,0 +1,118 @@ +"""Unit tests for riskenv.risk_assessment β€” calc_cpa and heading_from_quaternion.""" +import math +import pytest +from pytest import approx + +from riskenv import Agent, Obstacle, calc_cpa, heading_from_quaternion + + +# --------------------------------------------------------------------------- +# heading_from_quaternion +# --------------------------------------------------------------------------- + +class TestHeadingFromQuaternion: + def test_identity_quaternion_gives_zero_heading(self): + assert heading_from_quaternion(0, 0, 0, 1) == approx(0.0, abs=1e-9) + + def test_90_degrees_around_z(self): + # 90Β° yaw: qz = sin(45Β°), qw = cos(45Β°) + qz = math.sin(math.pi / 4) + qw = math.cos(math.pi / 4) + assert heading_from_quaternion(0, 0, qz, qw) == approx(math.pi / 2, abs=1e-9) + + def test_180_degrees_around_z(self): + qz = math.sin(math.pi / 2) + qw = math.cos(math.pi / 2) + assert abs(heading_from_quaternion(0, 0, qz, qw)) == approx(math.pi, abs=1e-9) + + def test_unnormalised_quaternion_is_handled(self): + # 2Γ— identity β€” should still give heading 0 + assert heading_from_quaternion(0, 0, 0, 2) == approx(0.0, abs=1e-9) + + +# --------------------------------------------------------------------------- +# calc_cpa +# --------------------------------------------------------------------------- + +class TestCalcCpa: + def _agent(self, **kw): + defaults = dict(position=(0.0, 0.0), heading=0.0, speed=10.0, + yaw_rate=0.0, safety_radius=1.0) + defaults.update(kw) + return Agent(**defaults) + + def _obs(self, **kw): + defaults = dict(position=(0.0, 0.0), heading=0.0, speed=0.0, + yaw_rate=0.0, safety_radius=1.0, tag="t") + defaults.update(kw) + return Obstacle(**defaults) + + # --- Future CPA cases --- + + def test_head_on_collision_dcpa_near_zero(self): + """Two objects approaching head-on should meet at dcpa β‰ˆ 0.""" + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(100.0, 0.0), heading=math.pi, speed=10.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert tcpa == approx(5.0, abs=1e-9) + assert dcpa == approx(0.0, abs=1e-9) + + def test_tcpa_positive_and_finite_for_approaching_objects(self): + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(50.0, 10.0), heading=math.pi, speed=5.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert tcpa > 0 + assert math.isfinite(tcpa) + assert math.isfinite(dcpa) + + def test_dcpa_and_tcpa_match_analytic_crossing_case(self): + """Agent going east, obstacle coming from the north β€” compute expected CPA.""" + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(0.0, 50.0), heading=-math.pi / 2, speed=10.0) + # p_rel=[0,-50], v_rel=[10,10] β†’ tcpa=2.5; cpa=[25,-25] β†’ dcpa=sqrt(1250) + dcpa, tcpa = calc_cpa(agent, obs) + assert tcpa == approx(2.5, abs=1e-9) + assert dcpa == approx(math.sqrt(1250), abs=1e-6) + + # --- Zero relative velocity --- + + def test_zero_relative_velocity_returns_current_distance(self): + """Same velocity β†’ dcpa equals the current separation.""" + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(0.0, 10.0), heading=0.0, speed=10.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert dcpa == approx(10.0, abs=1e-9) + + # --- Degenerate / past-CPA cases --- + + def test_identical_positions_same_velocity_returns_nan_dcpa_inf_tcpa(self): + """Same position, same velocity β†’ zero relative velocity β†’ nan, inf.""" + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(0.0, 0.0), heading=0.0, speed=10.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert math.isnan(dcpa) + assert math.isinf(tcpa) + + def test_identical_positions_different_velocity_returns_nan_nan(self): + """Same position, non-zero relative velocity β†’ tcpa=0 (already at CPA) β†’ nan, nan.""" + agent = self._agent(position=(0.0, 0.0), heading=0.0, speed=10.0) + obs = self._obs(position=(0.0, 0.0), heading=0.0, speed=0.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert math.isnan(dcpa) + assert math.isnan(tcpa) + + def test_past_cpa_returns_nan_nan(self): + """Objects moving apart β€” CPA is in the past.""" + agent = self._agent(position=(0.0, 0.0), heading=math.pi, speed=10.0) + obs = self._obs(position=(100.0, 0.0), heading=0.0, speed=10.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert math.isnan(dcpa) + assert math.isnan(tcpa) + + def test_3d_positions_use_xy_projection(self): + """z coordinates should be ignored in the CPA calculation.""" + agent = self._agent(position=(0.0, 0.0, 5.0), heading=0.0, speed=10.0) + obs = self._obs(position=(100.0, 0.0, 20.0), heading=math.pi, speed=10.0) + dcpa, tcpa = calc_cpa(agent, obs) + assert tcpa == approx(5.0, abs=1e-9) + assert dcpa == approx(0.0, abs=1e-9) diff --git a/tests/unit_tests/unsafe_set/test_unsafe_set.py b/tests/unit_tests/unsafe_set/test_unsafe_set.py index b188336..0276362 100644 --- a/tests/unit_tests/unsafe_set/test_unsafe_set.py +++ b/tests/unit_tests/unsafe_set/test_unsafe_set.py @@ -1,142 +1,73 @@ -# import pytest -# from unittest.mock import MagicMock, patch -# from src.colav_unsafe_set import create_unsafe_set -# from src.colav_unsafe_set.unsafe_set import _gen_uIoI_convhull, _generate_circle_vertices, _calc_dynamic_obstacles_tcpa_dcpa -# from src.colav_unsafe_set.indices_of_interest import ( -# calc_I1, -# calc_I2, -# calc_I3, -# unionise_indices_of_interest -# ) -# from src.colav_unsafe_set.objects import ( -# Configuration, -# DynamicObject, -# DynamicObstacle, -# DynamicObstacleWithMetrics -# ) -# from scipy.spatial import ConvexHull -# import numpy as np - - -# @pytest.fixture -# def agent_vessel(): -# # Mock a dynamic object for the agent vessel -# return MagicMock(spec=DynamicObject) - - -# @pytest.fixture -# def dynamic_obstacle(): -# # Mock a dynamic obstacle -# return MagicMock(spec=DynamicObstacle) - - -# @pytest.fixture -# def dynamic_obstacle_with_metrics(dynamic_obstacle): -# # Create a DynamicObstacleWithMetrics object -# return DynamicObstacleWithMetrics( -# dynamic_obstacle=dynamic_obstacle, dcpa=1.0, tcpa=2.0 -# ) - - -# def test_create_unsafe_set_no_dynamic_obstacles(agent_vessel): -# """Test if create_unsafe_set returns an empty list when no obstacles exist.""" -# dynamic_obstacles = [] -# dsf = 1.0 -# result = create_unsafe_set(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the result is an empty list -# assert result == [] - - -# def test_create_unsafe_set_with_dynamic_obstacles(agent_vessel, dynamic_obstacle): -# """Test the function with some dynamic obstacles.""" -# dynamic_obstacles = [dynamic_obstacle] -# dsf = 1.0 - -# # Mocking the functions that are called within create_unsafe_set -# with patch('_calc_dynamic_obstacles_tcpa_dcpa', return_value=[dynamic_obstacle]) as mock_calc_tcpa_dcpa, \ -# patch('calc_I1', return_value=[dynamic_obstacle]), \ -# patch('calc_I2', return_value=[dynamic_obstacle]), \ -# patch('calc_I3', return_value=[dynamic_obstacle]), \ -# patch('unionise_indices_of_interest', return_value=[dynamic_obstacle]): - -# result = create_unsafe_set(agent_vessel, dynamic_obstacles, dsf) - -# # Assert that the result is a list -# assert isinstance(result, list) - -# def test_generate_circle_vertices(): -# """Test if circle vertices are correctly generated.""" -# centroid = [0, 0] -# radius = 1.0 -# result = _generate_circle_vertices(centroid, radius) - -# # Assert the result is a list with correct number of points (default is 10) -# assert len(result) == 10 -# assert isinstance(result[0], list) -# assert len(result[0]) == 2 # Each point should be a 2D point (x, y) - - -# def test_gen_uIoI_convhull_with_no_vertices(): -# """Test _gen_uIoI_convhull with no vertices.""" -# result = _gen_uIoI_convhull([]) - -# # Assert that the result is an empty list -# assert result == [] - - -# def test_gen_uIoI_convhull_with_some_vertices(dynamic_obstacle_with_metrics): -# """Test _gen_uIoI_convhull with some valid dynamic obstacles.""" -# result = _gen_uIoI_convhull([dynamic_obstacle_with_metrics]) - -# # Assert that the result is a list (ConvexHull vertices indices) -# assert isinstance(result, list) - - -# def test_calc_dynamic_obstacles_tcpa_dcpa(dynamic_obstacle): -# """Test _calc_dynamic_obstacles_tcpa_dcpa.""" -# dynamic_obstacles = [dynamic_obstacle] -# agent_vessel = MagicMock(spec=DynamicObject) - -# with patch('yourmodule.calc_cpa', return_value=[1.0, 2.0]) as mock_calc_cpa: -# result = _calc_dynamic_obstacles_tcpa_dcpa(agent_vessel, dynamic_obstacles) - -# # Assert that the result contains the correct DynamicObstacleWithMetrics object -# assert isinstance(result[0], DynamicObstacleWithMetrics) -# assert result[0].dcpa == 1.0 -# assert result[0].tcpa == 2.0 - - -# def test_create_unsafe_set_with_no_obstacles_in_union(agent_vessel, dynamic_obstacle, dynamic_obstacle_with_metrics): -# """Test create_unsafe_set with no obstacles after unionisation.""" -# # Mock the functions to return empty results for unionise_indices_of_interest -# with patch('yourmodule._calc_dynamic_obstacles_tcpa_dcpa', return_value=[dynamic_obstacle_with_metrics]), \ -# patch('yourmodule.unionise_indices_of_interest', return_value=[]): - -# result = create_unsafe_set(agent_vessel, [dynamic_obstacle], 1.0) - -# # Assert that the result is an empty list (since there are no unionised obstacles) -# assert result == [] - - -# def test_create_unsafe_set_with_valid_union(agent_vessel, dynamic_obstacle_with_metrics): -# """Test create_unsafe_set with valid union.""" -# # Mock the necessary functions -# with patch('yourmodule._calc_dynamic_obstacles_tcpa_dcpa', return_value=[dynamic_obstacle_with_metrics]), \ -# patch('yourmodule.calc_I1', return_value=[dynamic_obstacle_with_metrics]), \ -# patch('yourmodule.calc_I2', return_value=[dynamic_obstacle_with_metrics]), \ -# patch('yourmodule.calc_I3', return_value=[dynamic_obstacle_with_metrics]), \ -# patch('yourmodule.unionise_indices_of_interest', return_value=[dynamic_obstacle_with_metrics]): - -# result = create_unsafe_set(agent_vessel, [dynamic_obstacle_with_metrics], 1.0) - -# # Assert that the result is a list (it should contain vertices of the convex hull) -# assert isinstance(result, list) - - -# def main(): -# pytest.main() - - -# if __name__ == '__main__': -# main() +"""Unit tests for riskenv.unsafe_set β€” create_unsafe_set.""" +import math +import pytest +from riskenv import Agent, Obstacle, create_unsafe_set + + +@pytest.fixture +def agent(): + return Agent(position=(0.0, 0.0), heading=0.0, speed=10.0, + yaw_rate=0.0, safety_radius=1.0) + + +class TestCreateUnsafeSet: + def test_no_obstacles_returns_empty(self, agent): + assert create_unsafe_set(agent=agent, obstacles=[], dsf=10.0) == [] + + def test_all_distant_obstacles_returns_empty(self, agent): + far = Obstacle(position=(200.0, 200.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=1.0, tag="far") + result = create_unsafe_set(agent=agent, obstacles=[far], dsf=5.0) + assert result == [] + + def test_nearby_approaching_obstacle_generates_hull(self, agent): + obs = Obstacle(position=(8.0, 0.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=2.0, tag="close") + result = create_unsafe_set(agent=agent, obstacles=[obs], dsf=10.0) + assert isinstance(result, list) + assert len(result) > 0 + + def test_hull_vertices_are_2d_pairs(self, agent): + obs = Obstacle(position=(8.0, 0.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=2.0) + result = create_unsafe_set(agent=agent, obstacles=[obs], dsf=10.0) + for vertex in result: + assert len(vertex) == 2 + + def test_hull_vertices_are_floats(self, agent): + obs = Obstacle(position=(8.0, 0.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=2.0) + result = create_unsafe_set(agent=agent, obstacles=[obs], dsf=10.0) + for x, y in result: + assert isinstance(x, float) + assert isinstance(y, float) + + def test_multiple_obstacles_generate_hull(self, agent): + obs_list = [ + Obstacle(position=(8.0, 0.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=2.0, tag="a"), + Obstacle(position=(0.0, 8.0), heading=-math.pi / 2, speed=5.0, + yaw_rate=0.0, safety_radius=2.0, tag="b"), + ] + result = create_unsafe_set(agent=agent, obstacles=obs_list, dsf=10.0) + assert len(result) >= 3 # convex hull of two circles has β‰₯ 3 vertices + + def test_time_of_interest_parameter_affects_result(self, agent): + """An obstacle with a large TCPA should be excluded when time_of_interest is small.""" + # Place an obstacle far enough that I1/I2 don't catch it, only I3 would + obs = Obstacle(position=(50.0, 0.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=1.0, tag="toi") + short_toi = create_unsafe_set(agent=agent, obstacles=[obs], dsf=5.0, time_of_interest=1.0) + long_toi = create_unsafe_set(agent=agent, obstacles=[obs], dsf=5.0, time_of_interest=600.0) + # With dsf=5 and obstacle at (50,0) adjusted distβ‰ˆ47, it's never in I1/I2. + # With very long toi and small dcpa it may enter I3 β€” results can differ. + assert isinstance(short_toi, list) + assert isinstance(long_toi, list) + + def test_2d_and_3d_agent_positions_work(self): + agent_3d = Agent(position=(0.0, 0.0, 5.0), heading=0.0, speed=10.0, + yaw_rate=0.0, safety_radius=1.0) + obs = Obstacle(position=(8.0, 0.0, 5.0), heading=math.pi, speed=5.0, + yaw_rate=0.0, safety_radius=2.0) + result = create_unsafe_set(agent=agent_3d, obstacles=[obs], dsf=10.0) + assert isinstance(result, list)