diff --git a/.dockerignore b/.dockerignore index 6ecc3f6b40..5632309535 100644 --- a/.dockerignore +++ b/.dockerignore @@ -34,12 +34,14 @@ **/.mypy_cache/ **/nosetests.xml **/OpEn_build/ +**/.panda3d_cache **/pip-delete-this-directory.txt **/pip-log.txt **/pip-wheel-metadata/ **/__pycache__ **/.pytest_cache **/.python-version +**/.pytype **/*.rou.alt.xml **/*.rou.xml **/*.sif @@ -54,7 +56,7 @@ **/.tox/ **/traffic_histories.pkl **/*.trips.xml -**/.venv +**/.venv* **/.vscode **/wheels/ **/xdummy.log diff --git a/.github/workflows/ci-auto-commit-linux.yml b/.github/workflows/ci-auto-commit-linux.yml index 1661ce1a8a..45790d4450 100644 --- a/.github/workflows/ci-auto-commit-linux.yml +++ b/.github/workflows/ci-auto-commit-linux.yml @@ -33,7 +33,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] - name: Update requirements run: | . ${{env.venv_dir}}/bin/activate diff --git a/.github/workflows/ci-auto-commit-mac.yml b/.github/workflows/ci-auto-commit-mac.yml index 266b54d9ba..ed20770121 100644 --- a/.github/workflows/ci-auto-commit-mac.yml +++ b/.github/workflows/ci-auto-commit-mac.yml @@ -28,7 +28,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] pip freeze | grep -v 'smarts' | grep -v 'pkg-resources==0.0.0' > utils/setup/mac_requirements.txt - name: Commit changes uses: EndBug/add-and-commit@v7 diff --git a/.github/workflows/ci-base-tests-linux.yml b/.github/workflows/ci-base-tests-linux.yml index f6b1e243fd..b15940a1a0 100644 --- a/.github/workflows/ci-base-tests-linux.yml +++ b/.github/workflows/ci-base-tests-linux.yml @@ -9,7 +9,7 @@ jobs: base-tests: runs-on: ubuntu-20.04 if: github.event_name == 'push' || github.event.pull_request.head.repo.full_name != github.repository - container: ghcr.io/smarts-project/smarts:v0.6.1-minimal + container: ghcr.io/smarts-project/smarts:v2.0.0-software_render strategy: matrix: tests: @@ -30,10 +30,11 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install -e .[camera_obs,opendrive,test,test_notebook,torch,train,gif_recorder,gymnasium,argoverse,envision,sumo] + pip install -e .[camera-obs,opendrive,test,test-notebook,torch,train,gif-recorder,gymnasium,argoverse,envision,sumo] if echo ${{matrix.tests}} | grep -q -e "test_rllib_hiway_env.py"; then pip install -e .[rllib]; fi if echo ${{matrix.tests}} | grep -q -e "test_examples.py"; then pip install -e .[examples,rllib]; fi if echo ${{matrix.tests}} | grep -q -e "/smarts/ray"; then pip install -e .[ray]; fi + if echo ${{matrix.tests}} | grep -q -e "/smarts/core"; then (/usr/bin/Xorg -noreset +extension GLX +extension RANDR +extension RENDER -logfile ./xdummy.log -config /etc/X11/xorg.conf -novtswitch :1 &); fi - name: Build scenarios run: | . ${{env.venv_dir}}/bin/activate @@ -55,6 +56,7 @@ jobs: --doctest-modules \ --forked \ --dist=no \ + --durations=10 \ -n auto \ --ignore-glob="**/ros.py" \ --ignore-glob="**/waymo_map.py" \ @@ -85,7 +87,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install -e ./../../.[camera_obs,argoverse,sumo,test] + pip install -e ./../../.[camera-obs,argoverse,sumo,test] pip install -e ./inference/ - name: Run smoke tests run: | @@ -117,7 +119,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install -e .[camera_obs,argoverse,test,ray,sumo] + pip install -e .[camera-obs,argoverse,test,ray,sumo] scl zoo install examples/${{matrix.tests}}/inference - name: Run smoke tests run: | diff --git a/.github/workflows/ci-base-tests-mac.yml b/.github/workflows/ci-base-tests-mac.yml index f207fca9eb..9e2bf542e2 100644 --- a/.github/workflows/ci-base-tests-mac.yml +++ b/.github/workflows/ci-base-tests-mac.yml @@ -21,7 +21,7 @@ jobs: - ./examples/tests --ignore=./examples/tests/test_learning.py - ./smarts/sstudio - ./smarts/env/tests/test_rllib_hiway_env.py - - ./smarts/core --nb-exec-timeout 65536 --ignore=./smarts/core/tests/test_notebook.py + - ./smarts/core --nb-exec-timeout 65536 --ignore=./smarts/core/tests/test_notebook.py --ignore=./smarts/core/tests/test_renderers.py::test_custom_shader_pass_buffers - ./smarts/env --ignore=./smarts/env/tests/test_rllib_hiway_env.py - ./smarts/ray steps: @@ -46,7 +46,7 @@ jobs: pip install --upgrade pip pip install wheel==0.38.4 pip install -r utils/setup/mac_requirements.txt - pip install -e .[camera_obs,opendrive,rllib,test,test_notebook,torch,train,argoverse,envision,sumo] + pip install -e .[camera-obs,opendrive,rllib,test,test-notebook,torch,train,argoverse,envision,sumo] if echo ${{matrix.tests}} | grep -q -e "/env"; then pip install -e .[rllib]; fi if echo ${{matrix.tests}} | grep -q -e "/examples"; then pip install -e .[examples,rllib]; fi if echo ${{matrix.tests}} | grep -q "/ray"; then pip install -e .[ray]; fi diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index f43c942903..7e0dcb1f28 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -59,7 +59,7 @@ jobs: python3.8 -m venv ${{env.venv_dir}} . ${{env.venv_dir}}/bin/activate pip install wheel==0.38.4 - pip install -e .[dev,camera_obs,train,test] + pip install -e .[dev,camera-obs,train,test] - name: Get changed files on branch since branching id: changed-files shell: bash @@ -98,7 +98,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip "setuptools<58.3.0" pip install wheel==0.38.4 - pip install .[camera_obs,doc,train,ray,envision,argoverse,opendrive,waymo,sumo] + pip install .[camera-obs,doc,train,ray,envision,argoverse,opendrive,waymo,sumo] cd ${GITHUB_WORKSPACE}/docs make html SPHINXOPTS="-W -T -E -n --keep-going -b spelling -b linkcheck" - name: Check build output diff --git a/.github/workflows/ci-pull-request.yml b/.github/workflows/ci-pull-request.yml index ebf0b05f41..fe3c8ca8c0 100644 --- a/.github/workflows/ci-pull-request.yml +++ b/.github/workflows/ci-pull-request.yml @@ -22,7 +22,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] - name: SMARTS benchmark run: | cd $GITHUB_WORKSPACE @@ -32,6 +32,6 @@ jobs: scl scenario build-all --clean ./scenarios pytest --benchmark-save=previous --benchmark-min-rounds=10 --benchmark-timer=time.process_time ./smarts/env/tests/test_benchmark.py git checkout - - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] scl scenario build-all --clean ./scenarios pytest --benchmark-compare=0001_previous --benchmark-compare-fail=mean:10% --benchmark-min-rounds=10 --benchmark-timer=time.process_time ./smarts/env/tests/test_benchmark.py diff --git a/.github/workflows/ci-python-version-test.yml b/.github/workflows/ci-python-version-test.yml index e3a7c1e7d5..122918eb88 100644 --- a/.github/workflows/ci-python-version-test.yml +++ b/.github/workflows/ci-python-version-test.yml @@ -27,4 +27,4 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install .[camera_obs,rllib,sumo,test,torch,train] \ No newline at end of file + pip install .[camera-obs,rllib,sumo,test,torch,train] \ No newline at end of file diff --git a/.github/workflows/ci-test-learning.yml b/.github/workflows/ci-test-learning.yml index 4bc716cea1..3bc301981c 100644 --- a/.github/workflows/ci-test-learning.yml +++ b/.github/workflows/ci-test-learning.yml @@ -27,7 +27,7 @@ jobs: . ${{env.venv_dir}}/bin/activate pip install --upgrade pip pip install wheel==0.38.4 - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] - name: Verify learning run: | cd $GITHUB_WORKSPACE diff --git a/.github/workflows/ci-test-long-determinism.yml b/.github/workflows/ci-test-long-determinism.yml index 77daf1e29f..b9ad6dc430 100644 --- a/.github/workflows/ci-test-long-determinism.yml +++ b/.github/workflows/ci-test-long-determinism.yml @@ -26,7 +26,7 @@ jobs: python3.8 -m venv ${{env.venv_dir}} . ${{env.venv_dir}}/bin/activate pip install --upgrade pip - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] - name: Verify long determinism run: | cd $GITHUB_WORKSPACE diff --git a/.github/workflows/ci-test-memory-growth.yml b/.github/workflows/ci-test-memory-growth.yml index 94e7dd7b3d..e0066860b6 100644 --- a/.github/workflows/ci-test-memory-growth.yml +++ b/.github/workflows/ci-test-memory-growth.yml @@ -28,7 +28,7 @@ jobs: pip install --upgrade pip pip install wheel==0.38.4 pip install pympler - pip install .[camera_obs,rllib,test,torch,train] + pip install .[camera-obs,rllib,test,torch,train] - name: Test memory growth run: | cd $GITHUB_WORKSPACE diff --git a/.gitignore b/.gitignore index 190aeba651..9b0856278c 100644 --- a/.gitignore +++ b/.gitignore @@ -66,6 +66,9 @@ target/ # VSCode .vscode +# panda3d +.panda3d_cache + # pyenv .python-version @@ -153,4 +156,5 @@ collected_observations/ **/diagnostic/reports/* # Experiments -outputs/ \ No newline at end of file +outputs/ +vaw/ \ No newline at end of file diff --git a/.readthedocs.yaml b/.readthedocs.yaml index 09f052d476..a82a2d3059 100644 --- a/.readthedocs.yaml +++ b/.readthedocs.yaml @@ -22,7 +22,7 @@ python: - method: pip path: . extra_requirements: - - camera_obs + - camera-obs - doc - train - ray diff --git a/CHANGELOG.md b/CHANGELOG.md index 357a09d609..ebd4d045d4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,8 @@ Copy and pasting the git commit messages is __NOT__ enough. - The following methods now exist explicitly `Vehicle.{add_sensor|detach_sensor|subscribed_to|sensor_property|}`. - Resources loaded with `load_yaml_config_with_substitution()` now substitute in SMARTS configuration with single squiggle bracket `${}` syntax. This is currently restricted to environment variable names prefixed with `"SMARTS_"`. This extends to benchmark configuration and vehicle configuration. - Default vehicle definitions can be now modified using `assets:default_vehicle_definitions_list`/`SMARTS_ASSSETS_DEFAULT_VEHICLE_DEFINITIONS_LIST` or by providing a `vehicle_definitions_list.yaml` in the scenario. These vehicle types are related to the `AgentInterface.vehicle_type` attribute. +- New `CustomRender` agent interface option added. This allows using `glsl` fragment scripts to generate images from camera textures and simulation buffers. +- New `ObfuscationMap` agent interface option added. This uses the `OccupancyGridMap` to help generate an image of ground viewable area from the ego vehicle's perspective. - There is now a centralized `TraCI` server mananger that can be used to prevent port collisions. It can be run using `python smarts.core.utils.sumo_server` and the use of the server can be enabled with `SMARTS_SUMO_TRACI_SERVE_MODE="central"`. ### Changed - `VehicleIndex.build_agent_vehicle()` no longer has `filename` and `surface_patches` parameters. @@ -30,6 +32,12 @@ Copy and pasting the git commit messages is __NOT__ enough. - `Vehicle.build_agent_vehicle()` and `Vehicle.build_social_vehicle()` moved to `VehicleIndex`. - `smarts.core.configuration.Configuration.get_settings()` now uses the `PyYAML` default instead of forcefully casting to `str`. - Added `AgentInterface.vehicle_class` which allows selection of a dynamics vehicle from the vehicle definitions list file. +- Waypoints now have a `position` property (which will eventually replace `pos`). +- You must now implement `act()` for any agent inheriting from `smarts.core.agent.Agent`. +- `FunctionAgent` is now no longer dynamically defined. +- `Vias.hit_via_points` is now a property. +- `ViaPoint` now has an attribute `hit` which determines if the point has been "collected". +- Dependencies switched back to using `-` instead of `_` (e.g. "camera-obs"). ### Deprecated - Module `smarts.core.models` is now deprecated in favour of `smarts.assets`. - Deprecated a few things related to vehicles in the `Scenario` class, including the `vehicle_filepath`, `tire_parameters_filepath`, and `controller_parameters_filepath`. The functionality is now handled through the vehicle definitions. @@ -57,6 +65,8 @@ Copy and pasting the git commit messages is __NOT__ enough. - The via sensor and trip meter sensor now work without a mission. - Fixed a bug with `VehicleIndex.attach_sensors_to_vehicle()` that would generate an invalid plan. - Fixed a bug where vehicle sensor meta attributes would reference the wrong vehicle. +- Resolved issue with road waypoints not showing waypoints if the horizon was larger than the start of the lane. +- Fixed an issue where `SMARTS.reset()` would be unable to render cameras. - Squashed TraCI "retrying" stdout messages. ### Removed ### Security @@ -126,6 +136,8 @@ Copy and pasting the git commit messages is __NOT__ enough. - Interest vehicles now show up in Envision. - Seed of `hiway-v1` env can be retrieved through a new property `seed`. - Added `TrafficEngineActor` to describe a scenario studio defined actor that is controlled by a traffic engine. +- Docker images from now on out base from ``ubuntu:focal``. +- A new Docker image has been added for software rendering for use cases where there is no display and GPU. ### Changed - Changed waypoints in sumo maps to use more incoming lanes into junctions. - Increased the cutoff radius for filtering out waypoints that are too far away in junctions in sumo maps. diff --git a/Makefile b/Makefile index 2d2a29888c..d647ddefff 100644 --- a/Makefile +++ b/Makefile @@ -7,6 +7,7 @@ test: build-all-scenarios --doctest-modules \ --forked \ --dist=loadscope \ + --durations=0 \ -n `expr \( \`nproc\` \/ 2 \& \`nproc\` \> 3 \) \| 2` \ --nb-exec-timeout 65536 \ ./examples/tests ./smarts/env ./envision ./smarts/core ./smarts/sstudio \ @@ -48,6 +49,7 @@ test-long-determinism: scl scenario build --clean scenarios/sumo/minicity PYTHONHASHSEED=42 pytest -v \ --forked \ + --durations=0 \ ./smarts/env/tests/test_determinism.py::test_long_determinism .PHONY: test-memory-growth @@ -63,7 +65,7 @@ test-memory-growth: build-all-scenarios .PHONY: benchmark benchmark: build-all-scenarios - pytest -v ./smarts/env/tests/test_benchmark.py + pytest -v --durations=0 ./smarts/env/tests/test_benchmark.py .PHONY: test-zoo test-zoo: build-all-scenarios diff --git a/docs/benchmarks/driving_smarts_2023_1.rst b/docs/benchmarks/driving_smarts_2023_1.rst index 780b60e2a2..d70f55ff88 100644 --- a/docs/benchmarks/driving_smarts_2023_1.rst +++ b/docs/benchmarks/driving_smarts_2023_1.rst @@ -256,7 +256,7 @@ Train $ source ./.venv/bin/activate $ pip install --upgrade pip $ pip install wheel==0.38.4 - $ pip install -e ./../../.[camera_obs,argoverse,envision,sumo] + $ pip install -e ./../../.[camera-obs,argoverse,envision,sumo] $ pip install -e ./inference/ + Train locally without visualization @@ -307,7 +307,7 @@ Evaluate $ source ./.venv/bin/activate $ pip install --upgrade pip $ pip install wheel==0.38.4 - $ pip install -e .[camera_obs,argoverse,envision,sumo] + $ pip install -e .[camera-obs,argoverse,envision,sumo] $ scl zoo install examples/e10_drive/inference # For Driving SMARTS 2023.1 $ scl benchmark run driving_smarts_2023_1 examples.e10_drive.inference:contrib-agent-v0 --auto-install diff --git a/docs/benchmarks/driving_smarts_2023_3.rst b/docs/benchmarks/driving_smarts_2023_3.rst index 48716ca78b..d37e7d52b6 100644 --- a/docs/benchmarks/driving_smarts_2023_3.rst +++ b/docs/benchmarks/driving_smarts_2023_3.rst @@ -234,7 +234,7 @@ Train $ source ./.venv/bin/activate $ pip install --upgrade pip $ pip install wheel==0.38.4 - $ pip install -e ./../../.[camera_obs,argoverse,envision,sumo] + $ pip install -e ./../../.[camera-obs,argoverse,envision,sumo] $ pip install -e ./inference/ + Train locally without visualization @@ -285,7 +285,7 @@ Evaluate $ source ./.venv/bin/activate $ pip install --upgrade pip $ pip install wheel==0.38.4 - $ pip install -e .[camera_obs,argoverse,envision,sumo] + $ pip install -e .[camera-obs,argoverse,envision,sumo] $ scl zoo install examples/e11_platoon/inference $ scl benchmark run driving_smarts_2023_3 examples.e11_platoon.inference:contrib-agent-v0 --auto-install diff --git a/docs/conf.py b/docs/conf.py index 70bcad2e94..a68811692b 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -60,7 +60,6 @@ "%s", ), } - # Configuring automated generation of api documentation. # See: https://github.com/sphinx-contrib/apidoc apidoc_module_dir = ".." @@ -72,6 +71,8 @@ "scenarios", "smarts/ros", "zoo/policies/interaction_aware_motion_prediction", + "smarts/waymo/waymo_open_dataset/protos", + "zoo/evaluation/metrics", ] apidoc_extra_args = [ "--force", @@ -126,6 +127,7 @@ ("py:class", "ObsType"), ("py:class", "smarts.env.gymnasium.wrappers.metric.utils.T"), ("py:class", "enum.Enum"), + ("py:class", "bc.BulletClient"), } nitpick_ignore_regex = { (r"py:.*", r"av2\..*"), diff --git a/docs/resources/faq.rst b/docs/resources/faq.rst index 6453402ddf..6fd5ee84d1 100644 --- a/docs/resources/faq.rst +++ b/docs/resources/faq.rst @@ -11,19 +11,33 @@ This is a list of frequently asked questions. Feel free to suggest new entries! 2. Where can I find debug logs? In most cases SMARTS debug logs are located at ``~/.smarts``. These can be helpful to diagnose problems. -3. Exception: Could not open window. - This may be due to some old dependencies of ``Panda3D``. Try the following instructions to solve it. +3. Exception: Could not open display. (Ubuntu) + This may be due to needing a display to render with a ``GL`` renderer backend setting. Try the following instructions to solve it. .. code-block:: bash - # Set DISPLAY - $ vim ~/.bashrc - $ export DISPLAY=":1" + # Set DISPLAY, can be as needed + $ echo export DISPLAY=":1" >> ~/.bashrc $ source ~/.bashrc - # Set xorg server + # Do once: Install x11 dummy which allows creating a fake display + $ sudo apt-get install -y xserver-xorg-video-dummy x11-apps + # Potentially the following if you need software rendering: + # sudo apt-get install -y mesa-utils + + # Do once: set xorg server $ sudo wget -O /etc/X11/xorg.conf http://xpra.org/xorg.conf - $ sudo /usr/bin/Xorg -noreset +extension GLX +extension RANDR +extension RENDER -logfile ./xdummy.log -config /etc/X11/xorg.conf $DISPLAY & 0 -4. The simulation keeps crashing on connection in ``SumoTrafficSimulation``. How do I fix this? + # Do as needed: + $ sudo /usr/bin/Xorg -noreset +extension GLX +extension RANDR +extension RENDER -logfile ./xdummy.log -config /etc/X11/xorg.conf $DISPLAY & + + + Note that ``mesa-utils`` installs ``llvm``, which is one option out of several that emulate ``OpenGL`` using software. ``llvm`` is not needed if a GPU is available. + +4. Custom rendering and Obfuscation maps show completely blank. (Ubuntu) + This is due to needing ``OpenGL`` to render using scripts. If you have a GPU make sure ``OpenGL`` is installed and the GPU has the necessary drivers for rendering. + + See the previous question if you need software rendering. + +5. The simulation keeps crashing on connection in ``SumoTrafficSimulation``. How do I fix this? This is likely due to using large scale parallelization. You will want to use the centralized management server. See :ref:`centralized_traci_management`. diff --git a/docs/setup.rst b/docs/setup.rst index 851ae5bef1..79c87c0a73 100644 --- a/docs/setup.rst +++ b/docs/setup.rst @@ -35,7 +35,7 @@ This includes SMARTS but none of the examples. $ bash utils/setup/install_deps.sh # This should install the latest version of SMARTS from package index (generally PyPI). - $ pip install 'smarts[camera_obs,sumo,example]' + $ pip install 'smarts[camera-obs,sumo,example]' Development @@ -79,13 +79,13 @@ Run the following commands to setup the SMARTS simulator. $ pip install --upgrade pip # Install smarts with extras as needed. Extras include the following: - # `camera_obs` - needed for rendering camera observations, and for testing. + # `camera-obs` - needed for rendering camera observations, and for testing. # `sumo` - needed for using SUMO scenarios. # `test` - needed for running tests. # `example` - needed for running examples. # `--config-settings editable_mode=strict` - may be needed depending on version of setuptools. # See https://github.com/huawei-noah/SMARTS/issues/2090. - $ pip install -e '.[camera_obs,sumo,test,example]' --config-settings editable_mode=strict + $ pip install -e '.[camera-obs,sumo,test,example]' --config-settings editable_mode=strict # Run sanity-test and verify they are passing. # If tests fail, check './sanity_test_result.xml' for test report. diff --git a/docs/sim/agent.rst b/docs/sim/agent.rst index 10b9b13818..f2695bc31f 100644 --- a/docs/sim/agent.rst +++ b/docs/sim/agent.rst @@ -159,7 +159,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - ✓ - ✓ - ✓ - - ✓ + - ✓ * - **neighborhood_vehicles** - ✓ - ✓ @@ -174,7 +174,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - ✓ - ✓ * - **waypoint_paths** - - ✓ + - ✓ - ✓ - ✓ - ✓ @@ -185,7 +185,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - ✓ - ✓ - ✓ - - + - * - **drivable_area_grid_map** - ✓ - @@ -198,11 +198,10 @@ The attributes enabled for each pre-configured `interface` is shown in the table - - - - - + - * - **occupancy_grid_map** - ✓ - - - - - - @@ -211,7 +210,8 @@ The attributes enabled for each pre-configured `interface` is shown in the table - - - - - + - + - * - **top_down_rgb** - ✓ - @@ -224,7 +224,33 @@ The attributes enabled for each pre-configured `interface` is shown in the table - - - - - + - + * - **occlusion_map** + - + - + - + - + - + - + - + - + - + - + - + - + * - **custom_renders** + - + - + - + - + - + - + - + - + - + - + - + - * - **lidar_point_cloud** - ✓ - @@ -234,10 +260,10 @@ The attributes enabled for each pre-configured `interface` is shown in the table - - - - - - - - - + - + - * - **accelerometer** - ✓ - ✓ @@ -250,7 +276,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - ✓ - ✓ - ✓ - - ✓ + - ✓ * - **signals** - ✓ - @@ -263,7 +289,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - - - - - ✓ + - ✓ * - **debug** - ✓ - ✓ @@ -276,7 +302,7 @@ The attributes enabled for each pre-configured `interface` is shown in the table - ✓ - ✓ - ✓ - - ✓ + - ✓ Here, ``max_episode_steps`` controls the max steps allowed for the agent in an episode. Defaults to ``None``, implies agent has no step limit. @@ -356,4 +382,238 @@ All `policies` must inherit the base class of :class:`~~smarts.core.agent.Agent` The received ``obs`` argument in ``def act(self, obs)`` is controlled by the selected agent `interface`. The ``act()`` method should return an action complying to the agent's chosen action type in its agent `interface`. -For example, if action type :attr:`~smarts.core.controllers.action_space_type.ActionSpaceType.LaneWithContinuousSpeed` was chosen, then ``act()`` should return an action ``(speed, lane_change)`` with type ``(float, int)``. See the :ref:`example ` above. \ No newline at end of file +For example, if action type :attr:`~smarts.core.controllers.action_space_type.ActionSpaceType.LaneWithContinuousSpeed` was chosen, then ``act()`` should return an action ``(speed, lane_change)`` with type ``(float, int)``. See the :ref:`example ` above. + +Custom camera rendering +----------------------- + +The agent interface provides for additional configurable rendering cameras that can be used to augment what the agent can see. +This generally involves providing a fragment shader to generate or modify existing or uninitialized pixels. + + +Configuring custom rendering +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +These custom renders can be configured through the agent interface. + +.. code:: python + + # adding a variable + # uniform vec2 position; + crcd = CustomRenderVariableDependency( + value=(4, 4), + variable_name="position" + ) + + # adding a buffer + # uniform float time; + crbd = CustomRenderBufferDependency( + buffer_dependency_name=BufferID.ELAPSED_SIM_TIME, + variable_name="time", + ) + + # referencing an inbuilt camera to + # uniform sampler2D iChannel0; + # uniform vec2 iChannel0Resolution; + crd0 = CustomRenderCameraDependency( + camera_dependency_name=CameraSensorID.OCCLUSION, + variable_name="iChannel0", + ) + # fragment shader + fshader0 = "warp.frag" + cr0 = CustomRender( + name="step0", + fragment_shader_path=fshader0, + dependencies=(crd0, crcd, crbd), + ..., + ) + # referencing a previous shader step + # uniform sampler2D step0_texture; + crd1 = CustomRenderCameraDependency( + camera_dependency_name="step0", + variable_name="step0_texture", + ) + # fragment shader + fshader1 = "ftl.frag" + cr1 = CustomRender( + name="step1", + fragment_shader_path=fshader1, # fragment shader + dependencies=(crd1,), + ..., + ) + + AgentInterface( + ..., + custom_renders=( + cr0, + cr1, + ) + ) + +.. note:: + + For camera dependencies a variable name will translate into both the texture sampler and resolution of the target camera. + + +Available internal render buffers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The render optionally has access to certain buffers. Note the following code: + +.. code:: python + + # adding a buffer + crbd = CustomRenderBufferDependency( + buffer_dependency_name=BufferID.DELTA_TIME, + variable_name="dt", + ) + + +These values are as follows: + +.. list-table:: + :header-rows: 1 + + * - **configuration** + - DELTA_TIME + - STEP_COUNT + - STEPS_COMPLETED + - ELAPSED_SIM_TIME + - EVENTS_COLLISIONS + - EVENTS_OFF_ROAD + - EVENTS_OFF_ROUTE + - EVENTS_ON_SHOULDER + - EVENTS_WRONG_WAY + - EVENTS_NOT_MOVING + - EVENTS_REACHED_GOAL + - EVENTS_REACHED_MAX_EPISODE_STEPS + - EVENTS_AGENTS_ALIVE_DONE + - EVENTS_INTEREST_DONE + - EGO_VEHICLE_STATE_POSITION + - EGO_VEHICLE_STATE_BOUNDING_BOX + - EGO_VEHICLE_STATE_HEADING + - EGO_VEHICLE_STATE_SPEED + - EGO_VEHICLE_STATE_STEERING + - EGO_VEHICLE_STATE_YAW_RATE + - EGO_VEHICLE_STATE_ROAD_ID + - EGO_VEHICLE_STATE_LANE_ID + - EGO_VEHICLE_STATE_LANE_INDEX + - EGO_VEHICLE_STATE_LINEAR_VELOCITY + - EGO_VEHICLE_STATE_ANGULAR_VELOCITY + - EGO_VEHICLE_STATE_LINEAR_ACCELERATION + - EGO_VEHICLE_STATE_ANGULAR_ACCELERATION + - EGO_VEHICLE_STATE_LINEAR_JERK + - EGO_VEHICLE_STATE_ANGULAR_JERK + - EGO_VEHICLE_STATE_LANE_POSITION + - UNDER_THIS_VEHICLE_CONTROL + - NEIGHBORHOOD_VEHICLE_STATES_POSITION + - NEIGHBORHOOD_VEHICLE_STATES_BOUNDING_BOX + - NEIGHBORHOOD_VEHICLE_STATES_HEADING + - NEIGHBORHOOD_VEHICLE_STATES_SPEED + - NEIGHBORHOOD_VEHICLE_STATES_ROAD_ID + - NEIGHBORHOOD_VEHICLE_STATES_LANE_ID + - NEIGHBORHOOD_VEHICLE_STATES_LANE_INDEX + - NEIGHBORHOOD_VEHICLE_STATES_LANE_POSITION + - NEIGHBORHOOD_VEHICLE_STATES_INTEREST + - WAYPOINT_PATHS_POSITION + - WAYPOINT_PATHS_HEADING + - WAYPOINT_PATHS_LANE_ID + - WAYPOINT_PATHS_LANE_WIDTH + - WAYPOINT_PATHS_SPEED_LIMIT + - WAYPOINT_PATHS_LANE_INDEX + - WAYPOINT_PATHS_LANE_OFFSET + - DISTANCE_TRAVELLED + - ROAD_WAYPOINTS_POSITION + - ROAD_WAYPOINTS_HEADING + - ROAD_WAYPOINTS_LANE_ID + - ROAD_WAYPOINTS_LANE_WIDTH + - ROAD_WAYPOINTS_SPEED_LIMIT + - ROAD_WAYPOINTS_LANE_INDEX + - ROAD_WAYPOINTS_LANE_OFFSET + - VIA_DATA_NEAR_VIA_POINTS_POSITION + - VIA_DATA_NEAR_VIA_POINTS_LANE_INDEX + - VIA_DATA_NEAR_VIA_POINTS_ROAD_ID + - VIA_DATA_NEAR_VIA_POINTS_REQUIRED_SPEED + - VIA_DATA_NEAR_VIA_POINTS_HIT + - LIDAR_POINT_CLOUD_POINTS + - LIDAR_POINT_CLOUD_HITS + - LIDAR_POINT_CLOUD_ORIGIN + - LIDAR_POINT_CLOUD_DIRECTION + - VEHICLE_TYPE + - SIGNALS_LIGHT_STATE + - SIGNALS_STOP_POINT + - SIGNALS_LAST_CHANGED + * - **type** + - uniform float + - uniform int + - uniform int + - uniform float + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform int + - uniform vec3 + - uniform vec3 + - uniform float + - uniform float + - uniform float + - uniform float + - uniform int + - uniform int + - uniform int + - uniform vec2 + - uniform vec2 + - uniform vec2 + - uniform vec2 + - uniform vec2 + - uniform vec2 + - uniform vec3 + - uniform int + - uniform vec3 + - uniform vec3 + - uniform float + - uniform float + - uniform int + - uniform int + - uniform int + - uniform vec3 + - uniform int + - uniform vec2 + - uniform float + - uniform int + - uniform float + - uniform float + - uniform int + - uniform float + - uniform float + - uniform vec2 + - uniform float + - uniform int + - uniform float + - uniform float + - uniform int + - uniform float + - uniform vec2 + - uniform int + - uniform int + - uniform float + - uniform int + - uniform vec3 + - uniform int + - uniform vec3 + - uniform vec3 + - uniform int + - uniform int + - uniform vec2 + - uniform float + + +.. note:: + + See :ref:`observation information ` for more information. \ No newline at end of file diff --git a/docs/sim/engine_configuration.rst b/docs/sim/engine_configuration.rst index d48b8999f0..d7ecbd9997 100644 --- a/docs/sim/engine_configuration.rst +++ b/docs/sim/engine_configuration.rst @@ -16,6 +16,8 @@ Configuration of the engine can come from several sources. These locations take 3. Local user engine configuration, ``~/.smarts/engine.ini``, if local directory configuration is not found. 4. Global engine configuration, ``/etc/smarts/engine.ini``, if local configuration is not found. 5. Package default configuration, ``$PYTHON_PATH/smarts/engine.ini``, if global configuration is not found. +6. Code default, ``config(section, option, default=4)``, if global configuration is not found. +7. Option default if code default is not given. Note that configuration files resolve all settings at the first found configuration file (they do not layer.) @@ -23,13 +25,23 @@ Note that configuration files resolve all settings at the first found configurat Options ------- -All settings demonstrated as environment variables are formatted to ``UPPERCASE`` and prefixed with ``SMARTS_`` (e.g. ``[core] logging`` can be configured with ``SMARTS_CORE_LOGGING``) +All settings demonstrated as environment variables are formatted to ``UPPERCASE`` and prefixed with ``SMARTS_`` + (e.g. ``[core] logging`` can be configured with ``SMARTS_CORE_LOGGING``). -These settings are as follows: +Below is a comparison of valid approaches to changing an engine configuration value: -.. todo:: +.. code:: ini - List engine settings + ; Example configuration file + ; For syntax see https://docs.python.org/3/library/configparser.html#supported-ini-file-structure + [assets] + default_agent_vehicle = passenger + + +.. code:: bash + + $ # Another way to apply the value + $ export SMARTS_ASSETS_DEFAULT_AGENT_VEHICLE=passenger YAML resources @@ -66,3 +78,127 @@ prefixed environment variables. is_debug: ${SMARTS_CORE_DEBUG} # literal environment variable or engine setting `[core] debug` + +Engine settings +--------------- + +The current list of engine settings are as follows: + +.. list-table:: + :header-rows: 1 + + * - **Setting** + - ``SMARTS_ASSETS_PATH`` + - ``SMARTS_ASSETS_DEFAULT_AGENT_VEHICLE`` + - ``SMARTS_ASSETS_DEFAULT_AGENT_DEFINITIONS_LIST`` + - ``SMARTS_CORE_DEBUG`` + - ``SMARTS_CORE_MAX_CUSTOM_IMAGE_SENSORS`` + - ``SMARTS_CORE_OBSERVATION_WORKERS`` + - ``SMARTS_CORE_RESET_RETRIES`` + - ``SMARTS_CORE_SENSOR_PARALLELIZATION`` + - ``SMARTS_PHYSICS_MAX_PYBULLET_FREQ`` + - ``SMARTS_RAY_NUM_CPUS`` + - ``SMARTS_RAY_NUM_GPUS`` + - ``SMARTS_RAY_LOG_TO_DRIVER`` + - ``SMARTS_SUMO_CENTRAL_HOST`` + - ``SMARTS_SUMO_TRACI_SERVE_MODE`` + - ``SMARTS_SUMO_CENTRAL_PORT`` + - ``SMARTS_VISDOM_ENABLED`` + - ``SMARTS_VISDOM_HOSTNAME`` + - ``SMARTS_VISDOM_PORT`` + * - **Section** + - assets + - assets + - assets + - core + - core + - core + - core + - core + - physics + - ray + - ray + - ray + - sumo + - sumo + - sumo + - visdom + - visdom + - visdom + * - **Type** + - string + - string + - string + - boolean + - integer + - integer + - integer + - string + - integer + - integer|``None`` + - integer|``None`` + - boolean + - string + - string + - integer + - boolean + - string + - integer + * - **Default** + - ``"/assets"`` + - ``"sedan"`` + - ``"/assets/vehicles/vehicle_definitions_list.yaml"`` + - ``False`` + - 32 + - 0 + - 0 + - ``"mp"`` + - 240 + - ``None`` + - 0 + - ``False`` + - 8619 + - ``"localhost"`` + - ``"local"`` + - False + - ``"http://localhost"`` + - 8097 + * - **Values** + - Any existing path (not recommended to change) + - Any defined vehicle name. + - Any existing ``YAML`` file. + - True|False + - 0 or greater + - 0 or greater (0 disables parallelization) + - 0 or greater + - [``"mp"`` ``"ray"``] + - 1 or greater (240 highly recommended) + - 0 or greater | None + - 0 or greater | None + - True|False + - [``"localhost"`` ``"x.x.x.x"`` ``"https://..."``] + - [``"local"`` ``"central"``] + - As dictated by OS. + - True|False + - [``localhost`` ``"x.x.x.x"`` ``"http://..."``] + - As dictated by OS. + * - **Description** + - The path to SMARTS package assets. + - This uses a vehicle from those defined in the ``SMARTS_ASSETS_DEFAULT_AGENT_DEFINITIONS_LIST`` file. + - The path to a vehicle definition file. See :ref:`vehicle defaults ` for more information. + - Enables additional debugging information from SMARTS. + - Reserves that number of custom image sensors for an individual vehicle. + - Determines how many workers SMARTS will use when generating observations. 0 disables parallelization. + - Increasing this value gives more attempts for SMARTS to reset to a valid initial state. This can be used to bypass edge case engine errors. + - Selects the parallelization backing for SMARTS sensors and observation generation. ``"mp"`` uses python's inbuilt ``"multiprocessing"`` library and ``"ray"`` uses `ray `. + - **WARNING** change at peril. Configures pybullet's frequency. + - Configures how many CPU's that ``ray`` will use. + - Configures how many GPU's that ``ray`` will use. + - Enables ``ray`` log debugging. + - If ``SMARTS_SUMO_SERVE_MODE=remote``, the host name of the remote ``TraCI`` management server host. + - If ``SMARTS_SUMO_SERVE_MODE=remote``, the port that the ``TraCI`` management server communicates on. + - The ``TraCI`` server spin-up mode to use. ``"local"`` generates the ``TraCI`` server from the local process. ``"remote"`` uses an intermediary server to generate ``TraCI`` servers and prevent race conditions between process connections. + - If to enable `visdom `_ visualization. + - The host name for the ``visdom`` instance. + - The port of the ``visdom`` instance. + diff --git a/docs/sim/obs_action_reward.rst b/docs/sim/obs_action_reward.rst index 24e0f1857d..11ca0e0b92 100644 --- a/docs/sim/obs_action_reward.rst +++ b/docs/sim/obs_action_reward.rst @@ -58,7 +58,7 @@ The complete set of possible :class:`~smarts.core.observations.Observation` retu Some observations like :attr:`~smarts.core.observations.Observation.occupancy_grid_map`, :attr:`~smarts.core.observations.Observation.drivable_area_grid_map`, and :attr:`~smarts.core.observations.Observation.top_down_rgb`, require the installation of optional packages for image rendering, so install them via - ``pip install -e .[camera_obs]``. + ``pip install -e .[camera-obs]``. Reward ------ diff --git a/docs/sim/visualization.rst b/docs/sim/visualization.rst index e0e5691ec5..3bbb4756ca 100644 --- a/docs/sim/visualization.rst +++ b/docs/sim/visualization.rst @@ -109,6 +109,13 @@ Enable ``visdom`` in the SMARTS environment by setting ``SMARTS_VISDOM_ENABLED`` enabled=True hostname="http://localhost" port=8097 + +Start the visdom server: + +.. code-block:: bash + + # 8097 is the default visdom port. + $ visdom -port 8097 Below is a sample visualization of an agent's camera sensor observations. diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt index ced97e6deb..e838cc56ee 100644 --- a/docs/spelling_wordlist.txt +++ b/docs/spelling_wordlist.txt @@ -42,6 +42,7 @@ instantiating instantiation instantiations iterable +kwargs lanepoint laner lidar @@ -68,6 +69,7 @@ serialize serializes serialized serializable +shader str superclass terminateds diff --git a/examples/e10_drive/inference/contrib_policy/filter_obs.py b/examples/e10_drive/inference/contrib_policy/filter_obs.py index 4be368171e..33dbbdf5b5 100644 --- a/examples/e10_drive/inference/contrib_policy/filter_obs.py +++ b/examples/e10_drive/inference/contrib_policy/filter_obs.py @@ -6,6 +6,7 @@ from smarts.core.agent_interface import RGB from smarts.core.colors import Colors, SceneColors +from smarts.core.utils.observations import points_to_pixels, replace_rgb_image_color class FilterObs: @@ -72,19 +73,19 @@ def filter(self, obs: Dict[str, Any]) -> Dict[str, Any]: # Get rgb image, remove road, and replace other egos (if any) as background vehicles rgb = obs["top_down_rgb"] h, w, _ = rgb.shape - rgb_noroad = replace_color(rgb=rgb, old_color=[self._road_color, self._lane_divider_color, self._edge_divider_color], new_color=self._no_color) - rgb_ego = replace_color(rgb=rgb_noroad, old_color=[self._ego_color], new_color=self._traffic_color, mask=self._rgb_mask) + rgb_noroad = replace_rgb_image_color(rgb=rgb, old_color=[self._road_color, self._lane_divider_color, self._edge_divider_color], new_color=self._no_color) + rgb_ego = replace_rgb_image_color(rgb=rgb_noroad, old_color=[self._ego_color], new_color=self._traffic_color, mask=self._rgb_mask) # Superimpose waypoints onto rgb image wps = obs["waypoint_paths"]["position"][0:11, 3:, 0:3] for path in wps[:]: wps_valid = points_to_pixels( points=path, - ego_pos=ego_pos, - ego_heading=ego_heading, - w=w, - h=h, - res=self._res, + center_position=ego_pos, + heading=ego_heading, + width=w, + height=h, + resolution=self._res, ) for point in wps_valid: img_x, img_y = point[0], point[1] @@ -95,11 +96,11 @@ def filter(self, obs: Dict[str, Any]) -> Dict[str, Any]: if not all((goal:=obs["ego_vehicle_state"]["mission"]["goal_position"]) == np.zeros((3,))): goal_pixel = points_to_pixels( points=np.expand_dims(goal,axis=0), - ego_pos=ego_pos, - ego_heading=ego_heading, - w=w, - h=h, - res=self._res, + center_position=ego_pos, + heading=ego_heading, + width=w, + height=h, + resolution=self._res, ) if len(goal_pixel) != 0: img_x, img_y = goal_pixel[0][0], goal_pixel[0][1] @@ -120,101 +121,3 @@ def filter(self, obs: Dict[str, Any]) -> Dict[str, Any]: return filtered_obs # fmt: on - - -def replace_color( - rgb: np.ndarray, - old_color: Sequence[np.ndarray], - new_color: np.ndarray, - mask: np.ndarray = np.ma.nomask, -) -> np.ndarray: - """Convert pixels of value `old_color` to `new_color` within the masked - region in the received RGB image. - - Args: - rgb (np.ndarray): RGB image. Shape = (m,n,3). - old_color (Sequence[np.ndarray]): List of old colors to be removed from the RGB image. Shape = (3,). - new_color (np.ndarray): New color to be added to the RGB image. Shape = (3,). - mask (np.ndarray, optional): Valid regions for color replacement. Shape = (m,n,3). - Defaults to np.ma.nomask . - - Returns: - np.ndarray: RGB image with `old_color` pixels changed to `new_color` - within the masked region. Shape = (m,n,3). - """ - # fmt: off - assert all(color.shape == (3,) for color in old_color), ( - f"Expected old_color to be of shape (3,), but got {[color.shape for color in old_color]}.") - assert new_color.shape == (3,), ( - f"Expected new_color to be of shape (3,), but got {new_color.shape}.") - - nc = new_color.reshape((1, 1, 3)) - nc_array = np.full_like(rgb, nc) - rgb_masked = np.ma.MaskedArray(data=rgb, mask=mask) - - rgb_condition = rgb_masked - result = rgb - for color in old_color: - result = np.ma.where((rgb_condition == color.reshape((1, 1, 3))).all(axis=-1)[..., None], nc_array, result) - - return result - # fmt: on - - -def points_to_pixels( - points: np.ndarray, - ego_pos: np.ndarray, - ego_heading: float, - w: int, - h: int, - res: float, -) -> np.ndarray: - """Converts points into pixel coordinates in order to superimpose the - points onto the RGB image. - - Args: - points (np.ndarray): Array of points. Shape (n,3). - ego_pos (np.ndarray): Ego position. Shape = (3,). - ego_heading (float): Ego heading in radians. - w (int): Width of RGB image - h (int): Height of RGB image. - res (float): Resolution of RGB image in meters/pixels. Computed as - ground_size/image_size. - - Returns: - np.ndarray: Array of point coordinates on the RGB image. Shape = (m,3). - """ - # fmt: off - mask = [False if all(point == np.zeros(3,)) else True for point in points] - points_nonzero = points[mask] - points_delta = points_nonzero - ego_pos - points_rotated = rotate_axes(points_delta, theta=ego_heading) - points_pixels = points_rotated / np.array([res, res, res]) - points_overlay = np.array([w / 2, h / 2, 0]) + points_pixels * np.array([1, -1, 1]) - points_rfloat = np.rint(points_overlay) - points_valid = points_rfloat[(points_rfloat[:,0] >= 0) & (points_rfloat[:,0] < w) & (points_rfloat[:,1] >= 0) & (points_rfloat[:,1] < h)] - points_rint = points_valid.astype(int) - return points_rint - # fmt: on - - -def rotate_axes(points: np.ndarray, theta: float) -> np.ndarray: - """A counterclockwise rotation of the x-y axes by an angle theta θ about - the z-axis. - - Args: - points (np.ndarray): x,y,z coordinates in original axes. Shape = (n,3). - theta (np.float): Axes rotation angle in radians. - - Returns: - np.ndarray: x,y,z coordinates in rotated axes. Shape = (n,3). - """ - # fmt: off - theta = (theta + np.pi) % (2 * np.pi) - np.pi - ct, st = np.cos(theta), np.sin(theta) - R = np.array([[ ct, st, 0], - [-st, ct, 0], - [ 0, 0, 1]]) - rotated_points = (R.dot(points.T)).T - return rotated_points - # fmt: on diff --git a/examples/e10_drive/train/Dockerfile b/examples/e10_drive/train/Dockerfile index 3e0a4cb3c3..ea7ef19826 100644 --- a/examples/e10_drive/train/Dockerfile +++ b/examples/e10_drive/train/Dockerfile @@ -33,7 +33,7 @@ RUN pip install --no-cache-dir -r /tmp/requirements.txt # Copy source files and install. COPY . /SMARTS WORKDIR /SMARTS -RUN pip install -e .[camera_obs,argoverse] +RUN pip install -e .[camera-obs,argoverse] RUN pip install -e ./examples/e10_drive/inference SHELL ["/bin/bash", "-c", "-l"] \ No newline at end of file diff --git a/examples/e11_platoon/train/Dockerfile b/examples/e11_platoon/train/Dockerfile index a77ab1cd7d..26c5888527 100644 --- a/examples/e11_platoon/train/Dockerfile +++ b/examples/e11_platoon/train/Dockerfile @@ -33,7 +33,7 @@ RUN pip install --no-cache-dir -r /tmp/requirements.txt # Copy source files and install. COPY . /SMARTS WORKDIR /SMARTS -RUN pip install -e .[camera_obs,argoverse] +RUN pip install -e .[camera-obs,argoverse] RUN pip install -e ./examples/e11_platoon/inference SHELL ["/bin/bash", "-c", "-l"] \ No newline at end of file diff --git a/examples/e9_notebook.ipynb b/examples/e9_notebook.ipynb index cdbf481ce6..30faff92ed 100644 --- a/examples/e9_notebook.ipynb +++ b/examples/e9_notebook.ipynb @@ -5,7 +5,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# SMARTS", + "# SMARTS\n", "This example demonstrates the use of SMARTS within a Jupyter notebook." ] }, @@ -33,8 +33,8 @@ "outputs": [], "source": [ "# !git clone https://github.com/huawei-noah/SMARTS 2> /dev/null\n", - "# !cd SMARTS && ls && pip install .[camera_obs]\n", - "%pip install smarts[camera_obs]" + "# !cd SMARTS && ls && pip install .[camera-obs]\n", + "%pip install smarts[camera-obs]" ] }, { diff --git a/examples/occlusion/A1_36_updated.jpg b/examples/occlusion/A1_36_updated.jpg new file mode 100644 index 0000000000..02db14e890 Binary files /dev/null and b/examples/occlusion/A1_36_updated.jpg differ diff --git a/examples/occlusion/mask.py b/examples/occlusion/mask.py new file mode 100644 index 0000000000..406bc826c1 --- /dev/null +++ b/examples/occlusion/mask.py @@ -0,0 +1,859 @@ +import importlib.resources as pkg_resources +import math +import os +import random +import subprocess +from abc import ABCMeta +from collections import defaultdict, deque +from dataclasses import replace +from functools import lru_cache, partial +from itertools import cycle, islice +from pathlib import Path +from typing import Any, Dict, Final, List, Optional, Sequence, Tuple, TypeVar, Union + +import matplotlib.pyplot as plt +import numpy as np +import shapely +from matplotlib import transforms +from PIL import Image +from shapely import prepared +from shapely.affinity import rotate as shapely_rotate +from shapely.geometry import GeometryCollection, MultiPolygon, Point, Polygon +from shapely.geometry import box as shapely_box +from shapely.ops import unary_union +from shapely.prepared import PreparedGeometry + +from smarts.core import glsl +from smarts.core.agent import Agent +from smarts.core.agent_interface import ( + OGM, + RGB, + Accelerometer, + AgentInterface, + CustomRender, + CustomRenderBufferDependency, + CustomRenderCameraDependency, + CustomRenderVariableDependency, + DoneCriteria, + DrivableAreaGridMap, + Lidar, + NeighborhoodVehicles, + OcclusionMap, + RoadWaypoints, + Waypoints, +) +from smarts.core.colors import Colors +from smarts.core.observations import Observation, VehicleObservation +from smarts.core.road_map import Waypoint, interpolate_waypoints +from smarts.core.shader_buffer import BufferID, CameraSensorID +from smarts.core.utils.core_math import slope, squared_dist +from smarts.core.utils.observations import points_to_pixels +from smarts.env.utils.observation_conversion import ObservationOptions + +T = TypeVar("T") +VIDEO_PREFIX: Final[str] = "A1_" +IMAGE_SUFFIX: Final[str] = "jpg" + + +OUTPUT_DIR: Final[Path] = Path("./vaw/vaw") + + +class PropertyAccessorUtil: + def __init__(self, mode: ObservationOptions) -> None: + if mode in (ObservationOptions.multi_agent, ObservationOptions.full): + # pytype: disable=unsupported-operands + self.position_accessor = lambda o: o["position"] + self.len_accessor = lambda o: o["box"][0] + self.width_accessor = lambda o: o["box"][1] + self.heading_accessor = lambda o: o["heading"] + self.ego_accessor = lambda o: o["ego_vehicle_state"] + self.nvs_accessor = lambda o: o["neighborhood_vehicle_states"] + self.wpp_accessor = lambda o: o["waypoint_paths"] + self.waypoint_position_accessor = lambda o: self.position_accessor( + self.wpp_accessor(o) + ) + # pytype: enable=unsupported-operands + else: + self.position_accessor = lambda o: o.position + self.len_accessor = lambda o: o.bounding_box.length + self.width_accessor = lambda o: o.bounding_box.width + self.heading_accessor = lambda o: o.heading + self.ego_accessor = lambda o: o.ego_vehicle_state + self.nvs_accessor = lambda o: o.neighborhood_vehicle_states + self.wpp_accessor = lambda o: o.waypoint_paths + + def _pad(waypoints): + max_lane_wps = max([len(lane) for lane in waypoints]) + base = np.zeros((len(waypoints), max_lane_wps, 3), dtype=np.float64) + + for i, lane in enumerate(waypoints): + for j, wp in enumerate(lane): + base[i, j, :2] = self.position_accessor(wp) + return base + + self.waypoint_position_accessor = lambda o: _pad(self.wpp_accessor(o)) + + +class PointGenerator: + @classmethod + @lru_cache(maxsize=1000) + def cache_generate(cls, x, y, *_): + return Point(x, y) + + +def generate_vehicle_polygon(position, length, width, heading) -> Polygon: + """Returns a bounding box around the vehicle.""" + half_len = 0.5 * length + half_width = 0.5 * width + poly = shapely_box( + position[0] - half_width, + position[1] - half_len, + position[0] + half_width, + position[1] + half_len, + ) + return shapely_rotate(poly, heading, use_radians=True) + + +def find_point_past_target(center, target_point, distance: float): + direction_vector = np.array( + (target_point[0] - center[0], target_point[1] - center[1]) + ) + ab_len = math.sqrt(direction_vector[0] ** 2 + direction_vector[1] ** 2) + unit_vector = direction_vector / ab_len + + return np.array( + (center[0] + unit_vector[0] * distance, center[1] + unit_vector[1] * distance) + ) + + +def perpendicular_slope(slope): + if slope == 0: # Special case for horizontal line + return math.inf + return -1 / slope + + +def get_midpoint(point1, point2): + return (np.array(point1) + np.array(point2)) / 2 + + +def is_edge_facing_away(edge_start, edge_end, reference_point): + edge_vector = np.array(edge_end) - np.array(edge_start) + perpendicular_vector = np.array([edge_vector[1], -edge_vector[0]]) + ref_vector = np.array(reference_point) - np.array(edge_end) + + dot_product = np.dot(perpendicular_vector, ref_vector) + + if dot_product > 0: + return True + elif dot_product < 0: + return False + else: + return None # Reference point lies on the edge + + +def generate_shadow_mask_polygons( + center, object_geometry: Polygon, radius: float +) -> List[Polygon]: + """From a center point cast a mask away from the object + + Args: + center (_type_): The source point of the cast. + object_geometry (_type_): The object to cast the mask from. Assumes counterclockwise coordinates. + radius (float): The distance from the source point to extent of the cast. + + Returns: + List[Polygon]: A series of masks from the sides of the geometry facing away from the center origin. + """ + out_shapes = [] + + facing_away_edges = deque() + for points in zip( + object_geometry.exterior.coords, + islice(cycle(object_geometry.exterior.coords), 1, 5), + ): + if is_edge_facing_away(points[0], points[1], center): + continue + + facing_away_edges.append(points) + + # Edges need to be rotated so that the coordinates are in strict sequential order rather than cycled + if len(facing_away_edges) > 0: + for i in range(len(facing_away_edges) - 1): + if facing_away_edges[i][1] == facing_away_edges[i + 1][0]: + continue + facing_away_edges.rotate(len(facing_away_edges) - (i + 1)) + break + + last_tangent_intersection = None + intersections = [] + point_a, point_b = None, None + for point_a, point_b in reversed(facing_away_edges): + ## If the intention is to generate a quadrilateral shadow cast towards the edge of the circle, + # the center point of the line must cast through to generate a tangential line at the circle edge to guarantee + # that each of the intersections from the bounding points of the original line to the tangent fall outside of the circle. + # Otherwise, the nearest point would need to be used. + midpoint = get_midpoint(point_a, point_b) + point_on_tangent = find_point_past_target(center, midpoint, radius) + tangent_slope = perpendicular_slope( + slope=slope( + center[0] - point_on_tangent[0], center[1] - point_on_tangent[1] + ) + ) + b2edge_intersection = find_tangent_intersection( + center, radius, point_b, point_on_tangent, tangent_slope + ) + intersection = b2edge_intersection + if last_tangent_intersection is not None and squared_dist( + center, b2edge_intersection + ) < squared_dist(center, last_tangent_intersection): + intersection = last_tangent_intersection + intersections.append(intersection[:2]) + a2edge_intersection = find_tangent_intersection( + center, radius, point_a, point_on_tangent, tangent_slope + ) + last_tangent_intersection = a2edge_intersection + + if point_a is not None and point_b is not None: + midpoint = get_midpoint(point_a, point_b) + point_on_tangent = find_point_past_target(center, midpoint, radius) + tangent_slope = perpendicular_slope( + slope=slope( + center[0] - point_on_tangent[0], + center[1] - point_on_tangent[1], + ) + ) + point_a, _ = facing_away_edges[0] + a2edge_intersection = find_tangent_intersection( + center, radius, point_a, point_on_tangent, tangent_slope + ) + intersections.append(a2edge_intersection[:2]) + + shell = [ + *(a for a, _ in facing_away_edges), + facing_away_edges[-1][-1], + *intersections, + ] + poly = Polygon(shell) + if poly.is_valid: + if isinstance(poly, (MultiPolygon, GeometryCollection)): + for g in poly.geoms: + if not hasattr(g, "exterior"): + continue + out_shapes.append(poly) + else: + out_shapes.append(poly) + else: + from shapely.validation import explain_validity + + print(explain_validity(poly)) + + return out_shapes + + +def find_tangent_intersection(center, radius, point_a, point_on_tangent, tangent_slope): + if squared_dist(point_a, center) > radius**2: + a2edge_intersection = point_on_tangent + else: + # This is the slope from the center to the near bounding points + a_slope = slope( + center[0] - point_a[0], + center[1] - point_a[1], + ) + # This is where the slope intersects with the circle tangent. + a2edge_intersection = find_line_intersection( + a_slope, tangent_slope, point1=center, point2=point_on_tangent + ) + + return a2edge_intersection + + +def generate_circle_polygon(center, radius) -> Polygon: + return PointGenerator.cache_generate(*center[:2]).buffer(radius) + + +def find_line_intersection(slope1, slope2, point1, point2): + x1, y1 = point1 + x2, y2 = point2 + + # Calculate the y-intercepts + b1 = y1 - slope1 * x1 + b2 = y2 - slope2 * x2 + + # Check if slopes are parallel (same slope with different intercepts) + if slope1 == slope2 and b1 != b2: + return None # No intersection (parallel lines) + + # Check if slopes are identical (same slope with same intercept) + if slope1 == slope2 and b1 == b2: + return math.inf, math.inf # Infinite intersection points + + # Calculate the x-coordinate of the intersection point + x = (b2 - b1) / (slope1 - slope2) + + # Calculate the y-coordinate of the intersection point + y = slope1 * x + b1 + + return x, y + + +def gen_circle_mask(center, radius): + circle_area = generate_circle_polygon(center, radius) + return circle_area + + +def gen_shadow_masks( + center, vehicle_states, radius, mode=ObservationOptions.multi_agent +): + if mode in (ObservationOptions.multi_agent, ObservationOptions.full): + accessor = lambda v: ( + v["position"], + *v["dimensions"][:2], # length, width + v["heading"], + ) + else: + accessor = lambda v: ( + v.position[:2], + *v.bounding_box.as_lwh[:2], + v.heading, + ) + masks: List[Polygon] = [] + for vehicle_state in vehicle_states: + vehicle_shape = generate_vehicle_polygon(*accessor(vehicle_state)) + new_masks = generate_shadow_mask_polygons(center, vehicle_shape, radius) + masks.append(unary_union(new_masks)) + return masks + + +def apply_masks( + center, vehicle_states: T, radius, mode=ObservationOptions.multi_agent +) -> T: + # Test that vehicles are within visible range + observation_area = gen_circle_mask(center, radius) + remaining_vehicle_states = [] + remaining_vehicle_points = [] + + if mode in (ObservationOptions.multi_agent, ObservationOptions.full): + gen = lambda vs: PointGenerator.cache_generate(*vs["position"]) + else: + gen = lambda vs: PointGenerator.cache_generate(*vs.position) + + for vehicle_state in vehicle_states: + position_point = gen(vehicle_state) + if not observation_area.contains(position_point): + continue + remaining_vehicle_states.append(vehicle_state) + remaining_vehicle_points.append(position_point) + + # Test that vehicles are not occluded + occlusion_masks = gen_shadow_masks(center, remaining_vehicle_states, radius, mode) + final_vehicle_states = [] + for vehicle_state, position_point in zip( + remaining_vehicle_states, remaining_vehicle_points + ): + # discard any vehicle state that is not included + for shadow_polygon in occlusion_masks: + if shadow_polygon.contains(position_point): + break # state is masked + else: + # if not masked + final_vehicle_states.append(vehicle_state) + return final_vehicle_states + + +def one_by_r_attenuation(*, dist2: float, **_): + """The crude formula for sound pressure attenuation. + + Args: + dist2 (float): The squared distance to attenuate. + """ + + return 1 / max(1, math.sqrt(abs(dist2))) + + +def one_by_r2_attenuation(*, dist2: float, **_): + """The crude formula for sound intensity attenuation. + + Args: + dist2 (float): The squared distance to attenuate. + """ + + return 1 / max(1, abs(dist2)) + + +def clamp(x, mn, mx): + return min(max(x, mn), mx) + + +def smoothstep(*, dist2: float, max_edge2: float, min_edge2: float = 0): + dist = max_edge2 - min_edge2 + x = clamp((dist2 - min_edge2) / dist, 0, 1) + + return x**2 * (3.0 - 2 * x) + + +def smootherstep(*, dist2: float, max_edge2: float, min_edge2: float = 0): + dist = max_edge2 - min_edge2 + x = clamp((dist2 - min_edge2) / dist, 0, 1) + + return x**3 * (3.0 * x * (2 * x - 5.0) + 10.0) + + +def one_minus_smoothstep(*, dist2: float, max_edge2: float, min_edge2: float = 0): + return 1 - smoothstep(dist2=dist2, max_edge2=max_edge2, min_edge2=min_edge2) + + +def one_minus_smootherstep(*, dist2: float, max_edge2: float, min_edge2: float = 0): + return 1 - smootherstep(dist2=dist2, max_edge2=max_edge2, min_edge2=min_edge2) + + +def gaussian_noise(base, mu=0, theta=0.15, sigma=0.3): + noise = theta * (mu - base) + sigma * np.random.randn(1) + return (base + noise)[0] + + +def gaussian_noise2(base, mu=0, sigma=0.078): + return base + random.gauss(mu, sigma) + + +def sample_weighted_binary_probability(normalized_bar): + bits_16 = int(0b111111111111111) + r = random.randint(0, bits_16) + + return r / bits_16 > normalized_bar + + +def certainty_displace( + center_point: Tuple[float, float], + target_point: Tuple[float, float], + perturb_target: T, + certainty_attenuation_fn=one_by_r2_attenuation, + uncertainty_noise_fn=gaussian_noise2, + coin_fn=sample_weighted_binary_probability, + max_sigma=0.073, + max_observable_radius: float = 10, +) -> Tuple[T, Dict[str, Any]]: + c_x, c_y = center_point + t_x, t_y = target_point + + d_x2 = (t_x - c_x) ** 2 + d_y2 = (t_y - c_y) ** 2 + + distance_attenuation = certainty_attenuation_fn( + dist2=d_x2 + d_y2, max_edge2=max_observable_radius**2 + ) + + # Coinflip chance of displacement + should_displace = coin_fn(normalized_bar=distance_attenuation) + + meta = dict(should_displace=should_displace, attenuation=distance_attenuation) + + if not should_displace: + return perturb_target, meta + + # Generate displacement + sigma = max_sigma * (1 - distance_attenuation) + meta["sigma"] = sigma + + if isinstance(perturb_target, (list, np.ndarray, Sequence)): + return ( + [uncertainty_noise_fn(pt_v, sigma=sigma) for pt_v in perturb_target], + meta, + ) + else: + return uncertainty_noise_fn(perturb_target, sigma=sigma), meta + + +_vehicle_states: List[Dict[str, Union[float, Tuple[float, float]]]] = [ + {"position": (1, 1), "dimensions": (2, 1), "heading": math.pi / 4}, + {"position": (-1, -1), "dimensions": (1.5, 0.8), "heading": math.pi / 2}, + {"position": (3, 2), "dimensions": (1.7, 0.9), "heading": 2 * math.pi / 3}, + {"position": (-2, 3), "dimensions": (1.3, 0.7), "heading": 7 * math.pi / 6}, + {"position": (0.5, -2), "dimensions": (1.8, 0.6), "heading": 5 * math.pi / 6}, + {"position": (-3, -2.5), "dimensions": (1.4, 0.7), "heading": math.pi}, + {"position": (2.5, -1), "dimensions": (1.6, 0.8), "heading": math.pi / 6}, + {"position": (-2, 1.5), "dimensions": (1.5, 0.8), "heading": 3 * math.pi / 4}, + {"position": (4, 3), "dimensions": (1.7, 0.9), "heading": math.pi / 3}, + {"position": (-3.5, -3), "dimensions": (1.3, 0.7), "heading": math.pi}, + {"position": (-3.5, 8), "dimensions": (1.3, 0.7), "heading": math.pi}, + {"position": (-13, 5), "dimensions": (1.3, 0.7), "heading": math.pi}, +] + + +def downgrade_waypoints( + center: Tuple[float, float], + waypoints: List[List[Waypoint]], + wp_space_resolution: float, + max_observable_radius: float, + waypoint_displacement_factor: float = 1e-5, + waypoint_heading_factor: float = math.pi * 0.125, +) -> List[List[Waypoint]]: + assert wp_space_resolution > 0, "Resolution must be a real number." + + center = center[:2] + + out_waypoints = defaultdict(list) + + for i, lane in enumerate(waypoints): + current_delta = 0 + for waypoint, n_waypoint in zip(lane[:-1], lane[1:]): + if waypoint.lane_offset - n_waypoint.lane_offset == 0: + continue + wps, used = interpolate_waypoints( + waypoint, + n_waypoint, + wp_space_resolution - current_delta % wp_space_resolution, + wp_space_resolution, + ) + current_delta += used + out_waypoints[i].extend(wps) + + return [ + [ + replace( + wp, + pos=certainty_displace( + center_point=center, + target_point=wp.pos, + perturb_target=wp.pos, + max_sigma=waypoint_displacement_factor, + certainty_attenuation_fn=one_minus_smootherstep, + max_observable_radius=max_observable_radius, + )[0], + heading=certainty_displace( + center_point=center, + target_point=wp.pos, + perturb_target=wp.heading, + max_sigma=waypoint_heading_factor, + max_observable_radius=max_observable_radius, + )[0], + ) + for wp in l + ] + for l in out_waypoints.values() + ] + + +def downgrade_vehicles( + center: Tuple[float, float], + neighborhood_vehicle_states: Tuple[VehicleObservation], + mode=ObservationOptions.multi_agent, +): + if mode: + pos_accessor = lambda o: o.position + heading_accessor = lambda o: o.heading + else: + pos_accessor = lambda o: o["position"] + heading_accessor = lambda o: o["heading"] + center = center[:2] + return [ + v._replace( + position=certainty_displace( + center_point=center, + target_point=pos_accessor(v)[:2], + perturb_target=pos_accessor(v), + max_sigma=2e-1, + )[0], + heading=certainty_displace( + center_point=center, + target_point=pos_accessor(v)[:2], + perturb_target=heading_accessor(v), + max_sigma=math.pi / 16, + )[0], + ) + for v in neighborhood_vehicle_states + ] + + +class AugmentationWrapper(Agent, metaclass=ABCMeta): + def __init__( + self, mode, observation_radius, output_dir, agent_name, record: bool + ) -> None: + self._observation_radius = observation_radius + self._output_dir = output_dir + self._agent_name = agent_name + self._mode = mode + self._pa = PropertyAccessorUtil(self._mode) + self._record = record + super().__init__() + + def export_video_image(self, ax, obs: Observation): + ego_state = self._pa.ego_accessor(obs) + _observation_center = self._pa.position_accessor(ego_state)[:2] + ax.plot(*PointGenerator.cache_generate(*_observation_center).xy, "r+") + xlim = self._observation_radius + 15 + ylim = xlim + ax.set_xlim(-xlim + _observation_center[0], xlim + _observation_center[0]) + ax.set_ylim(-ylim + _observation_center[1], ylim + _observation_center[1]) + if obs.steps_completed % 1 == 0: + ax.axis("off") + + plt.savefig( + self._output_dir + / f"{self._agent_name}_{obs.steps_completed}.{IMAGE_SUFFIX}" + ) + plt.close("all") + plt.cla() + + def generate_video(self, video_source_pattern="%d"): + if not self._record: + return + full_video_source_pattern = ( + f"{self._agent_name}_{video_source_pattern}.{IMAGE_SUFFIX}" + ) + video_name = f"{self._agent_name}.mp4" + os.chdir(self._output_dir) + subprocess.call( + [ + "ffmpeg", + "-framerate", + "8", + "-y", + "-i", + full_video_source_pattern, + "-r", + "30", + "-pix_fmt", + "yuv420p", + video_name, + ], + stderr=subprocess.DEVNULL, + ) + subprocess.call( + [ + "rm", + "-f", + f"{VIDEO_PREFIX}*.jpg", + ] + ) + subprocess.call(["mv", video_name, f"../{video_name}"]) + os.chdir("../..") + + +class OcclusionAgentWrapper(AugmentationWrapper): + def __init__( + self, + inner_agent, + mode: ObservationOptions, + agent_name, + observation_radius: float = 40.0, + scale: float = 1.0, + record: bool = True, + output_dir: Path = OUTPUT_DIR, + ) -> None: + self._inner_agent = inner_agent + self._wps_color = np.array(Colors.Green.value[:3]) * 255 + self._no_color = np.zeros(shape=(3,)) + self._inverse_scale = 1.0 / scale + os.makedirs(output_dir, exist_ok=True) + super().__init__( + mode=mode, + agent_name=agent_name, + output_dir=output_dir, + observation_radius=observation_radius, + record=record, + ) + + def act(self, obs: Optional[Observation], **configs): + if obs is None: + return None + assert obs.occlusion_map is not None + assert obs.drivable_area_grid_map is not None + + fig, ax = plt.subplots(subplot_kw=dict(aspect="equal")) + ax: plt.Axes + img_width, img_height = ( + obs.drivable_area_grid_map.metadata.width, + obs.drivable_area_grid_map.metadata.height, + ) + ego_state = self._pa.ego_accessor(obs) + ego_heading = self._pa.heading_accessor(ego_state) + _observation_center = self._pa.position_accessor(ego_state)[:2] + extent = [ + -img_width * 0.5, + img_width * 0.5, + -img_height * 0.5, + img_height * 0.5, + ] + + rgb_ego = obs.custom_renders[0].data.copy() + waypoint_paths = np.array(self._pa.waypoint_position_accessor(obs)) + + position = self._pa.position_accessor(ego_state) + heading = self._pa.heading_accessor(ego_state) + for path in waypoint_paths[0:11, 3:35, 0:3]: + wps_valid = points_to_pixels( + points=path, + center_position=position, + heading=heading, + width=obs.occlusion_map.metadata.width, + height=obs.occlusion_map.metadata.height, + resolution=obs.occlusion_map.metadata.resolution, + ) + for point in wps_valid: + img_x, img_y = point[0], point[1] + if all(rgb_ego[img_y, img_x, :] != self._no_color): + rgb_ego[img_y, img_x, :] = self._wps_color + else: + break + + if self._record: + tr = ( + transforms.Affine2D() + .rotate_deg(math.degrees(ego_heading)) + .translate(*_observation_center) + ) + ax.imshow( + rgb_ego, + # cmap="gray", + vmin=0, + vmax=255, + transform=tr + ax.transData, + extent=extent, + ) + + for vehicle in self._pa.nvs_accessor(obs): + vehicle_pos = np.array(self._pa.position_accessor(vehicle))[:2] + v_geom = generate_vehicle_polygon( + _observation_center + + (vehicle_pos - _observation_center) * self._inverse_scale, + self._pa.len_accessor(vehicle) * self._inverse_scale, + self._pa.width_accessor(vehicle) * self._inverse_scale, + self._pa.heading_accessor(vehicle), + ) + ax.plot(*v_geom.exterior.xy, color="b") + + self.export_video_image(ax, obs) + + dowgraded_obs = obs._replace() + return self._inner_agent.act(dowgraded_obs, **configs) + + +def occlusion_main(steps): + from smarts.env.gymnasium.hiway_env_v1 import HiWayEnvV1 + from smarts.zoo.registry import make + + observation_formatting = ObservationOptions.unformatted + + agent_spec = make("zoo.policies:keep-lane-agent-v0") + observation_radius = 60 + resolution = 1 + w, h = observation_radius * 2, observation_radius * 2 + agent_count = 1 + agents: Dict[str, OcclusionAgentWrapper] = { + f"A{c}": OcclusionAgentWrapper( + agent_spec.build_agent(), + observation_formatting, + observation_radius=observation_radius, + scale=resolution, + agent_name=f"A{c}", + ) + for c in range(1, agent_count + 1) + } + + with pkg_resources.path(glsl, "map_values_shader.frag") as frag_shader: + agent_interface: AgentInterface = replace( + agent_spec.interface, + neighborhood_vehicle_states=NeighborhoodVehicles(), + drivable_area_grid_map=DrivableAreaGridMap( + width=w, + height=h, + resolution=resolution, + ), + occupancy_grid_map=OGM( + width=w, + height=h, + resolution=resolution, + ), + top_down_rgb=RGB( + width=w, + height=h, + resolution=resolution, + ), + occlusion_map=OcclusionMap( + width=w, + height=h, + resolution=resolution, + surface_noise=True, + ), + signals=True, + lidar_point_cloud=Lidar(), + lane_positions=True, + accelerometer=Accelerometer(), + road_waypoints=RoadWaypoints(horizon=50), + waypoint_paths=Waypoints(lookahead=50), + done_criteria=DoneCriteria( + collision=False, off_road=False, off_route=False + ), + custom_renders=( + CustomRender( + name="noc", + fragment_shader_path=frag_shader, + dependencies=( + CustomRenderCameraDependency( + camera_dependency_name=CameraSensorID.OCCLUSION, + variable_name="iChannel0", + ), + CustomRenderCameraDependency( + camera_dependency_name=CameraSensorID.TOP_DOWN_RGB, + variable_name="iChannel1", + ), + CustomRenderBufferDependency( + buffer_dependency_name=BufferID.ELAPSED_SIM_TIME, + variable_name=BufferID.ELAPSED_SIM_TIME.value, + ), + CustomRenderBufferDependency( + buffer_dependency_name=BufferID.NEIGHBORHOOD_VEHICLE_STATES_POSITION, + variable_name=BufferID.NEIGHBORHOOD_VEHICLE_STATES_POSITION.value, + ), + CustomRenderVariableDependency( + value=(0.1, 0.5, 0.1), + variable_name="empty_color", + ), + ), + width=w, + height=h, + resolution=resolution, + ), + ), + ) + + with HiWayEnvV1( + scenarios=[ + # "./scenarios/sumo/intersections/4lane_t", + # "./smarts/diagnostic/n_sumo_actors/200_actors", + # "./scenarios/argoverse/straight/00a445fb-7293-4be6-adbc-e30c949b6cf7_agents_1/", + "./scenarios/argoverse/turn/0a60b442-56b0-46c3-be45-cf166a182b67_agents_1/", + # "./scenarios/argoverse/turn/0a764a82-b44e-481e-97e7-05e1f1f925f6_agents_1/", + # "./scenarios/argoverse/turn/0bf054e3-7698-4b86-9c98-626df2dee9f4_agents_1/", + ], + observation_options=observation_formatting, + action_options="unformatted", + agent_interfaces={"A1": agent_interface}, + fixed_timestep_sec=0.1, + ) as env: + terms = {"__all__": False} + obs, info = env.reset() + for _ in range(steps): + if terms["__all__"]: + break + acts = {a_id: a.act(obs.get(a_id)) for a_id, a in agents.items()} + + obs, rewards, terms, truncs, infos = env.step(acts) + + for _, a in agents.items(): + a.generate_video() + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser("Downgrader") + parser.add_argument( + "--steps", + help="The number of steps to take.", + type=int, + default=70, + ) + args = parser.parse_args() + + occlusion_main(args.steps) diff --git a/scenarios/sumo/intersections/4lane_t/agent_prefabs.py b/scenarios/sumo/intersections/4lane_t/agent_prefabs.py index 50da81c754..8c04d8ba48 100644 --- a/scenarios/sumo/intersections/4lane_t/agent_prefabs.py +++ b/scenarios/sumo/intersections/4lane_t/agent_prefabs.py @@ -15,7 +15,9 @@ def act(self, obs): class MotionPlannerAgent(Agent): def act(self, obs): wp = obs.waypoint_paths[0][:5][-1] - dist_to_wp = np.linalg.norm(wp.pos - obs.ego_vehicle_state.position[:2]) + dist_to_wp = np.linalg.norm( + np.subtract(wp.pos, obs.ego_vehicle_state.position[:2]) + ) target_speed = 5 # m/s return np.array([*wp.pos, wp.heading, dist_to_wp / target_speed]) diff --git a/scenarios/sumo/straight/3lane_bubble/agent_prefabs.py b/scenarios/sumo/straight/3lane_bubble/agent_prefabs.py index a2153fd51f..f4b4ec261a 100644 --- a/scenarios/sumo/straight/3lane_bubble/agent_prefabs.py +++ b/scenarios/sumo/straight/3lane_bubble/agent_prefabs.py @@ -13,7 +13,9 @@ def act(self, obs): def _single_act(self, obs): wp = obs.waypoint_paths[0][:5][-1] - dist_to_wp = np.linalg.norm(wp.pos - obs.ego_vehicle_state.position[:2]) + dist_to_wp = np.linalg.norm( + np.subtract(wp.pos, obs.ego_vehicle_state.position[:2]) + ) target_speed = 5 # m/s return np.array([*wp.pos, wp.heading, dist_to_wp / target_speed]) diff --git a/setup.cfg b/setup.cfg index 478a9bac62..3ddd60feed 100644 --- a/setup.cfg +++ b/setup.cfg @@ -49,7 +49,7 @@ console_scripts = argoverse = av2>=0.2.1 Rtree>=0.9.7 -camera_obs = +camera-obs = Panda3D>=1.10.13 panda3d-gltf==0.13 dev = @@ -80,7 +80,7 @@ examples = %(gymnasium)s %(sumo)s extras = pynput>=1.7.4 # Used by HumanKeyboardAgent -gif_recorder = +gif-recorder = moviepy == 1.0.3 gymnasium = gymnasium>=0.26.3 @@ -107,7 +107,7 @@ test = # The following are for testing pytest-cov>=3.0.0 pytest-xdist>=2.4.0 pytest-forked>=1.4.0 -test_notebook = +test-notebook = ipykernel>=4.10.1 jupyter-client>=7.1.2 pytest-notebook>=0.7.0 @@ -124,14 +124,14 @@ waymo = Rtree>=0.9.7 all = %(argoverse)s - %(camera_obs)s + %(camera-obs)s %(dev)s %(diagnostic)s %(doc)s %(envision)s %(examples)s %(extras)s - %(gif_recorder)s + %(gif-recorder)s %(gymnasium)s %(opendrive)s %(ray)s @@ -139,7 +139,7 @@ all = %(ros)s %(sumo)s %(test)s - %(test_notebook)s + %(test-notebook)s %(torch)s %(train)s %(visdom)s diff --git a/smarts/benchmark/entrypoints/benchmark_runner_v0.py b/smarts/benchmark/entrypoints/benchmark_runner_v0.py index 0068a80c2e..72fbb882cb 100644 --- a/smarts/benchmark/entrypoints/benchmark_runner_v0.py +++ b/smarts/benchmark/entrypoints/benchmark_runner_v0.py @@ -95,14 +95,12 @@ def _parallel_task_iterator(env_args, benchmark_args, agent_locator, *args, **_) requested_cpus: int = config()( "ray", "num_cpus", - None, - int, + cast=int, ) num_gpus = config()( "ray", "num_gpus", - 0, - float, + cast=float, ) num_cpus = ( requested_cpus @@ -114,8 +112,7 @@ def _parallel_task_iterator(env_args, benchmark_args, agent_locator, *args, **_) log_to_driver = config()( "ray", "log_to_driver", - False, - bool, + cast=bool, ) if num_cpus == 0 and num_gpus == 0: diff --git a/smarts/core/__init__.py b/smarts/core/__init__.py index 1d48622c5a..5694ce0fe0 100644 --- a/smarts/core/__init__.py +++ b/smarts/core/__init__.py @@ -63,7 +63,7 @@ def config( .. note:: This searches the following locations and loads the first one it finds: - Supplied config_path. default: `./smarts_engine.ini` + Supplied ``config_path``. Default: `./smarts_engine.ini` `~/.smarts/engine.ini` `/etc/smarts/engine.ini` `$PYTHON_PATH/smarts/engine.ini` diff --git a/smarts/core/actor.py b/smarts/core/actor.py index 9665aac02a..8815b3c553 100644 --- a/smarts/core/actor.py +++ b/smarts/core/actor.py @@ -17,10 +17,14 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations from dataclasses import dataclass from enum import IntEnum -from typing import Optional +from typing import TYPE_CHECKING, Optional + +if TYPE_CHECKING: + from smarts.core.coordinates import Dimensions, Pose class ActorRole(IntEnum): @@ -51,6 +55,14 @@ class ActorState: role: ActorRole = ActorRole.Unknown updated: bool = False + def get_pose(self) -> Optional[Pose]: + """Get the pose of this actor. Some actors do not have a physical location.""" + return None + + def get_dimensions(self) -> Optional[Dimensions]: + """Get the dimensions of this actor. Some actors do not have physical dimensions.""" + return None + def __lt__(self, other) -> bool: """Allows ordering ActorStates for use in sorted data-structures.""" assert isinstance(other, ActorState) diff --git a/smarts/core/actor_capture_manager.py b/smarts/core/actor_capture_manager.py index 60d984768a..fc03d5aa1b 100644 --- a/smarts/core/actor_capture_manager.py +++ b/smarts/core/actor_capture_manager.py @@ -26,27 +26,36 @@ from dataclasses import replace from typing import TYPE_CHECKING, Any, Dict, Optional -from smarts.core.plan import Plan +import smarts +from smarts.core.plan import NavigationMission, Plan from smarts.sstudio.sstypes import ConditionRequires if TYPE_CHECKING: + import smarts.core.scenario from smarts.core.actor import ActorState + from smarts.core.smarts import SMARTS from smarts.core.vehicle import Vehicle class ActorCaptureManager: """The base for managers that handle transition of control of actors.""" - def step(self, sim): + def step(self, sim: SMARTS): """Step the manager. Assume modification of existence and control of the simulation actors. Args: - sim (SMARTS): The smarts simulation instance. + sim (smarts.core.smarts.SMARTS): The smarts simulation instance. """ raise NotImplementedError() - def reset(self, scenario, sim): - """Reset this manager.""" + def reset(self, scenario: smarts.core.scenario.Scenario, sim: SMARTS): + """Reset this manager. + + :param scenario: The scenario to initialize from. + :type scenario: smarts.core.scenario.Scenario + :param sim: The simulation this is associated to. + :type scenario: smarts.core.smarts.SMARTS + """ raise NotImplementedError() def teardown(self): @@ -55,11 +64,8 @@ def teardown(self): @classmethod def _make_new_vehicle( - cls, sim, agent_id, mission, initial_speed, social=False + cls, sim: SMARTS, agent_id, mission, initial_speed, social=False ) -> Optional[Vehicle]: - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) if social: return cls.__make_new_social_vehicle(sim, agent_id, initial_speed) agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id) @@ -95,11 +101,12 @@ def __make_new_social_vehicle(sim, agent_id, initial_speed): @staticmethod def _take_existing_vehicle( - sim, vehicle_id, agent_id, mission, social=False + sim: SMARTS, + vehicle_id: str, + agent_id: str, + mission: NavigationMission, + social=False, ) -> Optional[Vehicle]: - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) if social: # MTA: TODO implement this section of actor capture. warnings.warn( @@ -118,8 +125,8 @@ def _take_existing_vehicle( def _gen_all_condition_kwargs( cls, agent_id: str, - mission, - sim, + mission: NavigationMission, + sim: SMARTS, actor_state: ActorState, condition_requires: ConditionRequires, ): @@ -133,7 +140,9 @@ def _gen_all_condition_kwargs( @staticmethod def _gen_mission_condition_kwargs( - agent_id: str, mission, condition_requires: ConditionRequires + agent_id: str, + mission: Optional[NavigationMission], + condition_requires: ConditionRequires, ) -> Dict[str, Any]: out_kwargs = dict() @@ -150,7 +159,7 @@ def _gen_mission_condition_kwargs( @staticmethod def _gen_simulation_condition_kwargs( - sim, condition_requires: ConditionRequires + sim: SMARTS, condition_requires: ConditionRequires ) -> Dict[str, Any]: out_kwargs = dict() diff --git a/smarts/core/agent.py b/smarts/core/agent.py index 9b0b3bee37..3e67ef09e2 100644 --- a/smarts/core/agent.py +++ b/smarts/core/agent.py @@ -19,12 +19,13 @@ # THE SOFTWARE. import logging import warnings +from abc import ABCMeta, abstractmethod from typing import Any, Callable logger = logging.getLogger(__name__) -class Agent: +class Agent(metaclass=ABCMeta): """The base class for agents""" @classmethod @@ -35,16 +36,9 @@ def from_function(cls, agent_function: Callable[[Any], Any]) -> "Agent": keep_lane_agent = Agent.from_function(lambda obs: "keep_lane") """ - assert callable(agent_function) - - class FunctionAgent(Agent): - """An agent generated from a function.""" - - def act(self, obs): - return agent_function(obs) - - return FunctionAgent() + return FunctionAgent(agent_function) + @abstractmethod def act(self, obs, **configs): """The agent action. See documentation on observations, `AgentSpec`, and `AgentInterface`. @@ -54,6 +48,17 @@ def act(self, obs, **configs): raise NotImplementedError +class FunctionAgent(Agent): + """An agent generated from a function.""" + + def __init__(self, agent_function) -> None: + assert callable(agent_function) + self._agent_function = agent_function + + def act(self, obs, **configs): + return self._agent_function(obs) + + def deprecated_agent_spec(*args, **kwargs): """Deprecated version of AgentSpec, see smarts.zoo.agent_spec""" from smarts.zoo.agent_spec import AgentSpec as AgentSpecAlias diff --git a/smarts/core/agent_interface.py b/smarts/core/agent_interface.py index bd8b514509..e2d8c674a1 100644 --- a/smarts/core/agent_interface.py +++ b/smarts/core/agent_interface.py @@ -17,16 +17,32 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + +import abc import re import warnings from dataclasses import dataclass, field, replace -from enum import IntEnum +from enum import Enum, IntEnum, auto from functools import cached_property -from typing import List, Literal, Optional, Tuple, Union +from pathlib import Path +from typing import TYPE_CHECKING, Any, List, Literal, Optional, Tuple, Union + +import numpy as np +from smarts.core import config from smarts.core.controllers.action_space_type import ActionSpaceType from smarts.core.lidar_sensor_params import BasicLidar from smarts.core.lidar_sensor_params import SensorParams as LidarSensorParams +from smarts.core.shader_buffer import BufferID, CameraSensorID +from smarts.core.utils import iteration_tools + + +class _DEFAULTS(Enum): + """The base defaults for the agent interface.""" + + SELF = "_SELF" + """Self-target the agent.""" @dataclass @@ -65,6 +81,124 @@ class RGB: resolution: float = 50 / 256 +@dataclass +class OcclusionMap: + """The width and height are in "pixels" and the resolution is the "size of a + pixel". E.g. if you wanted 100m x 100m `OcclusionMap` but a 64x64 image representation + you would do `OcclusionMap(width=64, height=64, resolution=100/64)` + """ + + width: int = 256 + height: int = 256 + resolution: float = 50 / 256 + surface_noise: bool = True + + +@dataclass +class RenderDependencyBase(metaclass=abc.ABCMeta): + """Base class for render dependencies""" + + @property + @abc.abstractmethod + def name(self) -> str: + """The name of this dependency.""" + raise NotImplementedError() + + +@dataclass +class CustomRenderVariableDependency(RenderDependencyBase): + """Base for renderer value to pass directly to the shader.""" + + value: Union[int, float, bool, np.ndarray, list, tuple] + """The value to pass to the shader.""" + variable_name: str + """The uniform name inside the shader.""" + + @property + def name(self) -> str: + return self.variable_name + + def __post_init__(self): + assert self.value, f"`{self.variable_name=}` cannot be None or empty." + assert self.variable_name + assert ( + 0 < len(self.value) < 5 + if isinstance(self.value, (np.ndarray, list, tuple)) + else True + ) + + +@dataclass +class CustomRenderBufferDependency(RenderDependencyBase): + """Base for referencing an observation buffer (other than a camera).""" + + buffer_dependency_name: Union[str, BufferID] + """The identification of buffer to reference.""" + variable_name: str + """The variable name inside the shader.""" + + target_actor: str = _DEFAULTS.SELF.value + + @property + def name(self) -> str: + return self.variable_name + + +@dataclass +class CustomRenderCameraDependency(RenderDependencyBase): + """Provides a uniform texture access to an existing camera.""" + + camera_dependency_name: Union[str, CameraSensorID] + """The name of the camera (type) to target.""" + variable_name: str + """The name of the camera texture variable.""" + + target_actor: str = _DEFAULTS.SELF.value + """The target actor to target. Defaults to targeting this vehicle.""" + + @property + def name(self) -> str: + return self.variable_name + + def is_self_targetted(self): + """If the dependency is drawing from one of this agent's cameras.""" + return self.target_actor == _DEFAULTS.SELF.value + + def __post_init__(self): + assert self.camera_dependency_name + if isinstance(self.camera_dependency_name, CameraSensorID): + self.camera_dependency_name = self.camera_dependency_name.value + + +@dataclass +class CustomRender: + """Utility render option that allows for a custom render output. + + The width and height are in "pixels" and the resolution is the "size of a + pixel". E.g. if you wanted 100m x 100m map but a 64x64 image representation + you would do `CustomRender(width=64, height=64, resolution=100/64)` + """ + + name: str + """The name used to generate the camera.""" + fragment_shader_path: Union[str, Path] + """The path string to the fragment shader.""" + dependencies: Tuple[RenderDependencyBase, ...] + """Inputs used by the fragment program.""" + width: int = 256 + """The number of pixels for the range of u.""" + height: int = 256 + """The number of pixels for the range of v.""" + resolution: float = 50 / 256 + """Scales the resolution to the given size. Resolution 1 matches width and height.""" + + def __post_init__(self): + assert len(self.dependencies) == len( + {d.name for d in self.dependencies} + ), f"Dependencies must have a unique name {list(iteration_tools.duplicates(self.dependencies, key=lambda d: d.name))}" + assert self.resolution > 0, "Resolution must result in at least 1 pixel." + + @dataclass class Lidar: """Lidar point cloud observations.""" @@ -90,8 +224,8 @@ class RoadWaypoints: that lane. """ - # The distance in meters to include waypoints for (both behind and in front of the agent) horizon: int = 20 + """The distance in meters to include waypoints for (both behind and in front of the agent)""" @dataclass @@ -106,15 +240,11 @@ class NeighborhoodVehicles: class Accelerometer: """Requires detection of motion changes within the agents vehicle.""" - pass - @dataclass class LanePositions: """Computation and reporting of lane-relative RefLine (Frenet) coordinates for all vehicles.""" - pass - @dataclass class Signals: @@ -343,6 +473,14 @@ class AgentInterface: Enable the signals sensor. """ + occlusion_map: Union[OcclusionMap, bool] = False + """Enable the `OcclusionMap` for the current vehicle. This image represents what the current vehicle can see. + """ + + custom_renders: Tuple[CustomRender, ...] = tuple() + """Add custom renderer outputs. + """ + def __post_init__(self): self.neighborhood_vehicle_states = AgentInterface._resolve_config( self.neighborhood_vehicle_states, NeighborhoodVehicles @@ -382,8 +520,29 @@ def __post_init__(self): self.vehicle_class = self.vehicle_type assert isinstance(self.vehicle_class, str) + assert len(self.custom_renders) <= config()( + "core", "max_custom_image_sensors", cast=int + ) + + self.occlusion_map = AgentInterface._resolve_config( + self.occlusion_map, OcclusionMap + ) + if self.occlusion_map: + assert self.occupancy_grid_map and isinstance( + self.occupancy_grid_map, OGM + ), "Occupancy grid map must also be attached to use occlusion map." + assert isinstance( + self.occlusion_map, OcclusionMap + ), "Occlusion map should have resolved to the datatype." + assert ( + self.occupancy_grid_map.width == self.occlusion_map.width + ), "Occlusion map width must match occupancy grid map." + assert ( + self.occupancy_grid_map.height == self.occlusion_map.height + ), "Occlusion map height must match occupancy grid map." + @staticmethod - def from_type(requested_type: AgentType, **kwargs): + def from_type(requested_type: AgentType, **kwargs) -> AgentInterface: """Instantiates from a selection of agent_interface presets Args: @@ -483,7 +642,7 @@ def from_type(requested_type: AgentType, **kwargs): return interface.replace(**kwargs) - def replace(self, **kwargs): + def replace(self, **kwargs) -> AgentInterface: """Clone this AgentInterface with the given fields updated >>> interface = AgentInterface(action=ActionSpaceType.Continuous) \ .replace(waypoint_paths=True) @@ -492,6 +651,16 @@ def replace(self, **kwargs): """ return replace(self, **kwargs) + @property + def requires_rendering(self): + """If this agent interface requires a renderer.""" + return bool( + self.top_down_rgb + or self.occupancy_grid_map + or self.drivable_area_grid_map + or self.custom_renders + ) + @property def ogm(self): """Deprecated. Use `occupancy_grid_map` instead.""" @@ -542,10 +711,10 @@ def neighborhood_vehicles(self): return self.neighborhood_vehicle_states @staticmethod - def _resolve_config(config, type_): - if config is True: + def _resolve_config(_config, type_) -> Union[Any, Literal[False]]: + if _config is True: return type_() - elif isinstance(config, type_): - return config + elif isinstance(_config, type_): + return _config else: return False diff --git a/smarts/core/agent_manager.py b/smarts/core/agent_manager.py index ed1b39d08a..de8331e810 100644 --- a/smarts/core/agent_manager.py +++ b/smarts/core/agent_manager.py @@ -17,26 +17,30 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations import logging import weakref from concurrent import futures -from typing import Any, Callable, Dict, List, Optional, Set, Tuple, Union +from typing import TYPE_CHECKING, Any, Callable, Dict, List, Optional, Set, Tuple, Union from envision.etypes import format_actor_id from smarts.core.actor import ActorRole -from smarts.core.agent_interface import AgentInterface from smarts.core.bubble_manager import BubbleManager from smarts.core.data_model import SocialAgent from smarts.core.local_agent_buffer import LocalAgentBuffer -from smarts.core.observations import Observation from smarts.core.plan import Plan -from smarts.core.sensor_manager import SensorManager from smarts.core.utils.id import SocialAgentId from smarts.core.vehicle_state import VehicleState from smarts.sstudio.sstypes.actor.social_agent_actor import SocialAgentActor from smarts.zoo.registry import make as make_social_agent +if TYPE_CHECKING: + from smarts.core.agent_interface import AgentInterface + from smarts.core.observations import Observation + from smarts.core.sensor_manager import SensorManager + from smarts.core.smarts import SMARTS + class AgentManager: """Tracks agent states and implements methods for managing agent life cycle. @@ -46,7 +50,7 @@ class AgentManager: time. """ - def __init__(self, sim, interfaces): + def __init__(self, sim: SMARTS, interfaces: Dict[str, AgentInterface]): from smarts.core.vehicle_index import VehicleIndex self._log = logging.getLogger(self.__class__.__name__) @@ -54,7 +58,7 @@ def __init__(self, sim, interfaces): self._vehicle_index: VehicleIndex = sim.vehicle_index self._sensor_manager: SensorManager = sim.sensor_manager self._agent_buffer = None - self._ego_agent_ids = set() + self._ego_agent_ids: Set[str] = set() self._social_agent_ids = set() # Initial interfaces are for agents that are spawned at the beginning of the @@ -62,11 +66,11 @@ def __init__(self, sim, interfaces): # agents and social agents defined in SStudio. Hijacking agents in bubbles # would not be included self._initial_interfaces = interfaces - self._pending_agent_ids = set() - self._pending_social_agent_ids = set() + self._pending_agent_ids: Set[str] = set() + self._pending_social_agent_ids: Set[str] = set() # Agent interfaces are interfaces for _all_ active agents - self._agent_interfaces = {} + self._agent_interfaces: Dict[str, AgentInterface] = {} # TODO: This field is only for social agents, but is being used as if it were # for any agent. Revisit the accessors. @@ -111,7 +115,7 @@ def agent_interfaces(self) -> Dict[str, AgentInterface]: """A list of all agent to agent interface mappings.""" return self._agent_interfaces - def agent_interface_for_agent_id(self, agent_id) -> AgentInterface: + def agent_interface_for_agent_id(self, agent_id: str) -> AgentInterface: """Get the agent interface of a specific agent.""" return self._agent_interfaces[agent_id] @@ -135,24 +139,24 @@ def shadowing_agent_ids(self) -> Set[str]: """Get all agents that currently observe, but not control, a vehicle.""" return self._vehicle_index.shadower_ids() - def is_ego(self, agent_id) -> bool: + def is_ego(self, agent_id: str) -> bool: """Test if the agent is an ego agent.""" return agent_id in self.ego_agent_ids - def remove_pending_agent_ids(self, agent_ids): + def remove_pending_agent_ids(self, agent_ids: Set[str]): """Remove an agent from the group of agents waiting to enter the simulation.""" assert agent_ids.issubset(self.agent_ids) self._pending_agent_ids -= agent_ids - def agent_for_vehicle(self, vehicle_id) -> str: + def agent_for_vehicle(self, vehicle_id: str) -> str: """Get the controlling agent for the given vehicle.""" return self._vehicle_index.owner_id_from_vehicle_id(vehicle_id) - def agent_has_vehicle(self, agent_id) -> bool: + def agent_has_vehicle(self, agent_id: str) -> bool: """Test if an agent has an actor associated with it.""" return len(self.vehicles_for_agent(agent_id)) > 0 - def vehicles_for_agent(self, agent_id) -> List[str]: + def vehicles_for_agent(self, agent_id: str) -> List[str]: """Get the vehicles associated with an agent.""" return self._vehicle_index.vehicle_ids_by_owner_id( agent_id, include_shadowers=True @@ -218,7 +222,7 @@ def observe( Dict[str, Union[Dict[str, bool], bool]], ]: """Generate observations from all vehicles associated with an active agent.""" - sim = self._sim() + sim: Optional[SMARTS] = self._sim() assert sim observations = {} rewards = {} @@ -340,7 +344,7 @@ def _vehicle_reward(self, vehicle_id: str) -> float: def _vehicle_score(self, vehicle_id: str) -> float: return self._vehicle_index.vehicle_by_id(vehicle_id).trip_meter_sensor() - def _filter_for_active_ego(self, dict_): + def _filter_for_active_ego(self, dict_: Dict[str, Observation]): return { id_: dict_[id_] for id_ in self._ego_agent_ids @@ -694,7 +698,9 @@ def _teardown_agents_by_ids(self, agent_ids, filter_ids: Set): self._pending_agent_ids = self._pending_agent_ids - ids_ return ids_ - def reset_agents(self, observations: Dict[str, Observation]): + def reset_agents( + self, observations: Dict[str, Observation] + ) -> Dict[str, Observation]: """Reset agents, feeding in an initial observation.""" self._send_observations_to_social_agents(observations) diff --git a/smarts/core/argoverse_map.py b/smarts/core/argoverse_map.py index 12eb039e16..f308d8eca8 100644 --- a/smarts/core/argoverse_map.py +++ b/smarts/core/argoverse_map.py @@ -439,14 +439,14 @@ def width_at_offset(self, lane_point_s: float) -> Tuple[float, float]: closest_index = np.argmin(dists) p1 = self.left_pts[closest_index] p2 = self.right_pts[closest_index] - width = np.linalg.norm(p2 - p1) + width = np.linalg.norm(np.subtract(p2, p1)) return width, 1.0 @cached_property def length(self) -> float: length = 0 for p1, p2 in zip(self._centerline, self._centerline[1:]): - length += np.linalg.norm(p2 - p1) + length += np.linalg.norm(np.subtract(p2, p1)) return length @cached_property @@ -1019,7 +1019,8 @@ def waypoint_paths( for path in new_paths: if ( len(path) > 0 - and np.linalg.norm(path[0].pos - pose.position[:2]) < 8 + and np.linalg.norm(np.subtract(path[0].pos, pose.position[:2])) + < 8 and abs(path[0].heading.relative_to(pose.heading)) < np.pi / 3 ): waypoint_paths.append(path) diff --git a/smarts/core/bezier_motion_planner.py b/smarts/core/bezier_motion_planner.py index ab4fe3be1d..d9885e94e5 100644 --- a/smarts/core/bezier_motion_planner.py +++ b/smarts/core/bezier_motion_planner.py @@ -106,7 +106,7 @@ def curve_lengths(subsections, t, p0, p1, p2, p3): # accumulate position deltas [s[t+1] - s[t]] for (ps, ps1) in zip(points[:-1], points[1:]): # convert deltas to magnitudes - delta_dist = ps1 - ps + delta_dist = np.subtract(ps1, ps) # add magnitudes subsection_length = np.linalg.norm(delta_dist) # add to lengths diff --git a/smarts/core/bubble_manager.py b/smarts/core/bubble_manager.py index bdf228171c..c81acc234c 100644 --- a/smarts/core/bubble_manager.py +++ b/smarts/core/bubble_manager.py @@ -35,7 +35,7 @@ from smarts.core.data_model import SocialAgent from smarts.core.plan import ( EndlessGoal, - Mission, + NavigationMission, Plan, PlanningError, PositionalGoal, @@ -859,7 +859,9 @@ def _prepare_sensors_for_agent_control( goal = PositionalGoal.from_road(dest_road_id, sim.scenario.road_map) else: goal = EndlessGoal() - mission = Mission(start=Start(vehicle.position[:2], vehicle.heading), goal=goal) + mission = NavigationMission( + start=Start(vehicle.position[:2], vehicle.heading), goal=goal + ) try: plan.create_route(mission) except PlanningError: diff --git a/smarts/core/chassis.py b/smarts/core/chassis.py index b4946ff65f..f6639facf4 100644 --- a/smarts/core/chassis.py +++ b/smarts/core/chassis.py @@ -17,12 +17,14 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import importlib.resources as pkg_resources import logging import math import os from functools import cached_property -from typing import Optional, Sequence, Tuple +from typing import TYPE_CHECKING, Any, Dict, Optional, Sequence, Tuple import numpy as np import yaml @@ -49,7 +51,10 @@ vec_to_radians, yaw_from_quaternion, ) -from smarts.core.utils.pybullet import bullet_client as bc + +if TYPE_CHECKING: + from smarts.core.utils.pybullet import bullet_client as bc + with pkg_resources.path( smarts.assets.vehicles.dynamics_model, "generic_sedan.urdf" @@ -69,7 +74,9 @@ DEFAULT_CHASSIS_PARAMETERS = yaml.safe_load(chassis_file) -def _query_bullet_contact_points(bullet_client, bullet_id, link_index): +def _query_bullet_contact_points( + bullet_client: bc.BulletClient, bullet_id: str, link_index: int +): contact_objects = set() # Give 0.05 meter leeway LEEWAY = 0.05 @@ -163,7 +170,7 @@ def yaw_rate(self) -> float: """The turning rate of the chassis in radians.""" raise NotImplementedError - def inherit_physical_values(self, other: "Chassis"): + def inherit_physical_values(self, other: Chassis): """Apply GCD between the two chassis.""" raise NotImplementedError @@ -329,7 +336,7 @@ def inherit_physical_values(self, other: Chassis): self.speed = other.speed # ignore physics - def step(self, current_simulation_time): + def step(self, current_simulation_time: float): pass def teardown(self): @@ -346,12 +353,12 @@ def __init__( self, pose: Pose, bullet_client: bc.BulletClient, - vehicle_dynamics_filepath=DEFAULT_VEHICLE_FILEPATH, - tire_parameters_filepath=None, - friction_map=None, - controller_parameters=DEFAULT_CONTROLLER_PARAMETERS, - chassis_parameters=DEFAULT_CHASSIS_PARAMETERS, - initial_speed=None, + vehicle_dynamics_filepath: Optional[str] = DEFAULT_VEHICLE_FILEPATH, + tire_parameters_filepath: Optional[str] = None, + friction_map: Optional[Sequence[Dict[str, Any]]] = None, + controller_parameters: Dict[str, Any] = DEFAULT_CONTROLLER_PARAMETERS, + chassis_parameters: Dict[str, Any] = DEFAULT_CHASSIS_PARAMETERS, + initial_speed: Optional[float] = None, ): assert isinstance(pose, Pose) self._log = logging.getLogger(self.__class__.__name__) diff --git a/smarts/core/configuration.py b/smarts/core/configuration.py index 8e381513d4..2914b9aa45 100644 --- a/smarts/core/configuration.py +++ b/smarts/core/configuration.py @@ -64,17 +64,21 @@ def _convert_truthy(t: str) -> bool: _assets_path, "vehicles/vehicle_definitions_list.yaml" ), ("core", "observation_workers"): 0, - ("core", "max_custom_image_sensors"): 4, + ("core", "max_custom_image_sensors"): 32, ("core", "sensor_parallelization"): "mp", ("core", "debug"): False, ("core", "reset_retries"): 0, - # ("physics", "max_pybullet_freq"): 240, + ("physics", "max_pybullet_freq"): 240, ("ray", "num_gpus"): 0, ("ray", "num_cpus"): None, ("ray", "log_to_driver"): False, ("sumo", "central_port"): 8619, ("sumo", "central_host"): "localhost", ("sumo", "traci_serve_mode"): "local", # local|central + ("traffic", "traci_retries"): 5, + ("visdom", "enabled"): False, + ("visdom", "hostname"): "http://localhost", + ("visdom", "port"): 8097, } @@ -153,7 +157,7 @@ def get_setting( value = self._config[section][option] except (configparser.NoSectionError, KeyError) as exc: if default is _UNSET: - if value := _config_defaults.get((section, option)): + if (value := _config_defaults.get((section, option), _UNSET)) != _UNSET: return value raise EnvironmentError( f"Setting `${env_variable}` cannot be found in environment or configuration." diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py index bf2983c0c8..4daa71e881 100644 --- a/smarts/core/controllers/lane_following_controller.py +++ b/smarts/core/controllers/lane_following_controller.py @@ -367,7 +367,7 @@ def perform_lane_following( def find_current_lane(wp_paths, vehicle_position): """Find the lane of the vehicle given a set of waypoint paths.""" relative_distant_lane = [ - np.linalg.norm(wp_paths[idx][0].pos - vehicle_position[0:2]) + np.linalg.norm(np.subtract(wp_paths[idx][0].pos, vehicle_position[:2])) for idx in range(len(wp_paths)) ] return np.argmin(relative_distant_lane) diff --git a/smarts/core/coordinates.py b/smarts/core/coordinates.py index d0cb7ecdb7..565ed9d859 100644 --- a/smarts/core/coordinates.py +++ b/smarts/core/coordinates.py @@ -17,10 +17,12 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import math from dataclasses import dataclass from functools import cached_property -from typing import Any, NamedTuple, Optional, SupportsFloat, Tuple, Union +from typing import Any, Collection, NamedTuple, Optional, SupportsFloat, Tuple, Union import numpy as np from shapely.geometry import Point as SPoint @@ -42,8 +44,8 @@ class Dimensions(NamedTuple): @classmethod def init_with_defaults( - cls, length: float, width: float, height: float, defaults: "Dimensions" - ) -> "Dimensions": + cls, length: float, width: float, height: float, defaults: Dimensions + ) -> Dimensions: """Create with the given default values""" if not length or length == -1: length = defaults.length @@ -54,9 +56,7 @@ def init_with_defaults( return cls(length, width, height) @classmethod - def copy_with_defaults( - cls, dims: "Dimensions", defaults: "Dimensions" - ) -> "Dimensions": + def copy_with_defaults(cls, dims: Dimensions, defaults: Dimensions) -> Dimensions: """Make a copy of the given dimensions with a default option.""" return cls.init_with_defaults(dims.length, dims.width, dims.height, defaults) @@ -86,11 +86,11 @@ class Point(NamedTuple): z: Optional[float] = 0 @classmethod - def from_np_array(cls, np_array: np.ndarray): + def from_np_array(cls, np_array: np.ndarray) -> Point: """Factory for constructing a Point object from a numpy array.""" assert 2 <= len(np_array) <= 3 z = np_array[2] if len(np_array) > 2 else 0.0 - return cls(np_array[0], np_array[1], z) + return cls(float(np_array[0]), float(np_array[1]), float(z)) @property def as_np_array(self) -> np.ndarray: @@ -215,7 +215,9 @@ class Heading(float): def __init__(self, value=...): float.__init__(value) - def __new__(self, x: Union[SupportsFloat, SupportsIndex, Ellipsis.__class__] = ...): + def __new__( + self, x: Union[SupportsFloat, SupportsIndex, Ellipsis.__class__] = ... + ) -> Heading: """A override to constrain heading to -pi to pi""" value = x if isinstance(value, (int, float)): @@ -227,7 +229,7 @@ def __new__(self, x: Union[SupportsFloat, SupportsIndex, Ellipsis.__class__] = . return float.__new__(self, value) @classmethod - def from_bullet(cls, bullet_heading): + def from_bullet(cls, bullet_heading) -> Heading: """Bullet's space is in radians, 0 faces north, and turns counter-clockwise. """ @@ -236,7 +238,7 @@ def from_bullet(cls, bullet_heading): return h @classmethod - def from_panda3d(cls, p3d_heading): + def from_panda3d(cls, p3d_heading) -> Heading: """Panda3D's space is in degrees, 0 faces north, and turns counter-clockwise. """ @@ -245,7 +247,7 @@ def from_panda3d(cls, p3d_heading): return h @classmethod - def from_sumo(cls, sumo_heading): + def from_sumo(cls, sumo_heading) -> Heading: """Sumo's space uses degrees, 0 faces north, and turns clockwise.""" heading = Heading.flip_clockwise(math.radians(sumo_heading)) h = Heading(heading) @@ -258,21 +260,21 @@ def source(self) -> Optional[str]: return getattr(self, "_source", None) @property - def as_panda3d(self): + def as_panda3d(self) -> float: """Convert to Panda3D facing format.""" return math.degrees(self) @property - def as_bullet(self): + def as_bullet(self) -> Heading: """Convert to bullet physics facing format.""" return self @property - def as_sumo(self): + def as_sumo(self) -> float: """Convert to SUMO facing format""" return math.degrees(Heading.flip_clockwise(self)) - def relative_to(self, other: "Heading"): + def relative_to(self, other: Heading) -> Heading: """ Computes the relative heading w.r.t. the given heading >>> Heading(math.pi/4).relative_to(Heading(math.pi)) @@ -286,7 +288,7 @@ def relative_to(self, other: "Heading"): return Heading(rel_heading) - def direction_vector(self): + def direction_vector(self) -> np.ndarray: """Convert to a 2D directional vector that aligns with Cartesian Coordinate System""" return radians_to_vec(self) @@ -340,6 +342,8 @@ def reset_with(self, position, heading: Heading): if "point" in self.__dict__: # clear the cached_property del self.__dict__["point"] + if "position_tuple" in self.__dict__: + del self.__dict__["position_tuple"] if heading != self.heading_: self.orientation = fast_quaternion_from_angle(heading) self.heading_ = heading @@ -347,10 +351,15 @@ def reset_with(self, position, heading: Heading): @cached_property def point(self) -> Point: """The positional value of this pose as a point.""" - return Point(*self.position) + return Point.from_np_array(self.position) + + @cached_property + def position_tuple(self) -> Tuple[Optional[float], ...]: + """The position value of this pose as a tuple.""" + return tuple(self.point) @classmethod - def from_front_bumper(cls, front_bumper_position, heading, length) -> "Pose": + def from_front_bumper(cls, front_bumper_position, heading, length) -> Pose: """Convert from front bumper location to a Pose with center of vehicle. Args: @@ -371,7 +380,7 @@ def from_front_bumper(cls, front_bumper_position, heading, length) -> "Pose": ) @classmethod - def from_center(cls, base_position, heading: Heading): + def from_center(cls, base_position: Collection[float], heading: Heading) -> Pose: """Convert from centered location Args: @@ -396,7 +405,7 @@ def from_explicit_offset( base_position: np.ndarray, heading: Heading, local_heading: Heading, - ): + ) -> Pose: """Convert from an explicit offset Args: @@ -441,12 +450,12 @@ def as_sumo(self, length, local_heading): self.heading.as_sumo, ) - def as_bullet(self): + def as_bullet(self) -> Tuple[np.ndarray, np.ndarray]: """Convert to bullet origin (position of bullet origin, orientation quaternion""" return (self.position, self.orientation) @property - def heading(self): + def heading(self) -> Heading: """The heading value converted from orientation.""" # XXX: changing the orientation should invalidate this @@ -460,11 +469,11 @@ def as_position2d(self) -> np.ndarray: """Convert to a 2d position array""" return self.position[:2] - def as_panda3d(self): + def as_panda3d(self) -> Tuple[np.ndarray, float]: """Convert to panda3D (object bounds center position, heading)""" return (self.position, self.heading.as_panda3d) @classmethod - def origin(cls): + def origin(cls) -> Pose: """Pose at the origin coordinate of smarts.""" return cls(np.repeat([0], 3), np.array([0, 0, 0, 1])) diff --git a/smarts/core/data_model.py b/smarts/core/data_model.py index 8a8e4b395e..b19044876c 100644 --- a/smarts/core/data_model.py +++ b/smarts/core/data_model.py @@ -17,12 +17,16 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + from dataclasses import dataclass, field -from typing import Any, Dict, Optional +from typing import TYPE_CHECKING, Any, Dict, Optional -from smarts.zoo.agent_spec import AgentSpec from smarts.zoo.registry import make +if TYPE_CHECKING: + from smarts.zoo.agent_spec import AgentSpec + @dataclass class SocialAgent: diff --git a/smarts/core/default_map_builder.py b/smarts/core/default_map_builder.py index 04e729c03f..30d89ad5bf 100644 --- a/smarts/core/default_map_builder.py +++ b/smarts/core/default_map_builder.py @@ -17,27 +17,32 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations import os import sys +import warnings from enum import IntEnum -from typing import NamedTuple, Optional, Tuple +from typing import TYPE_CHECKING, NamedTuple, Optional, Tuple -from smarts.core.road_map import RoadMap from smarts.core.utils.file import file_md5_hash, path2hash +if TYPE_CHECKING: + from smarts.core.road_map import RoadMap + from smarts.sstudio.sstypes import MapSpec + + _existing_map = None -def _cache_result(map_spec, road_map, road_map_hash: str): - global _existing_map - from smarts.sstudio.sstypes import MapSpec +class _RoadMapInfo(NamedTuple): + map_spec: MapSpec # pytype: disable=invalid-annotation + obj: RoadMap + map_hash: str - class _RoadMapInfo(NamedTuple): - map_spec: MapSpec # pytype: disable=invalid-annotation - obj: RoadMap - map_hash: str +def _cache_result(map_spec, road_map: RoadMap, road_map_hash: str): + global _existing_map _existing_map = _RoadMapInfo(map_spec, road_map, road_map_hash) @@ -151,6 +156,10 @@ def get_road_map(map_spec) -> Tuple[Optional[RoadMap], Optional[str]]: return None, None map_class = ArgoverseMap else: + warnings.warn( + f"A map source for road surface generation can not be resolved from the given reference: `{map_spec.source}`.", + category=UserWarning, + ) return None, None if _existing_map: diff --git a/smarts/core/events.py b/smarts/core/events.py index 405f6d636a..e5d408393a 100644 --- a/smarts/core/events.py +++ b/smarts/core/events.py @@ -17,14 +17,19 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import warnings -from typing import NamedTuple, Sequence +from typing import TYPE_CHECKING, NamedTuple, Tuple + +if TYPE_CHECKING: + from smarts.core.vehicle_state import Collision class Events(NamedTuple): """Classified observations that can trigger agent done status.""" - collisions: Sequence # Sequence[Collision] + collisions: Tuple[Collision] """Collisions with other vehicles (if any).""" off_road: bool """True if vehicle is off the road, else False.""" diff --git a/smarts/core/external_provider.py b/smarts/core/external_provider.py index 184012989d..9392603e6f 100644 --- a/smarts/core/external_provider.py +++ b/smarts/core/external_provider.py @@ -17,15 +17,19 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import weakref -from typing import Iterable, List, Sequence, Set +from typing import TYPE_CHECKING, Iterable, List, Sequence, Set from .actor import ActorRole from .controllers import ActionSpaceType from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .scenario import Scenario from .utils.file import replace -from .vehicle_state import VehicleState + +if TYPE_CHECKING: + from .vehicle_state import VehicleState class ExternalProvider(Provider): diff --git a/smarts/core/glsl/dashed_line_shader.frag b/smarts/core/glsl/dashed_line_shader.frag index da55292709..fb77c84ed6 100644 --- a/smarts/core/glsl/dashed_line_shader.frag +++ b/smarts/core/glsl/dashed_line_shader.frag @@ -6,14 +6,14 @@ flat in vec3 StartPos; in vec3 VertPos; uniform vec4 p3d_Color; -uniform vec2 Resolution; +uniform vec2 iResolution; void main() { uint pattern = 0x00FFu; float factor = 2.0; - vec2 dir = (VertPos.xy - StartPos.xy) * Resolution / 2.0; + vec2 dir = (VertPos.xy - StartPos.xy) * iResolution / 2.0; float dist = length(dir); uint bit = uint(round(dist / factor)) & 15U; diff --git a/smarts/core/glsl/map_values_shader.frag b/smarts/core/glsl/map_values_shader.frag new file mode 100644 index 0000000000..75d9dc1bcd --- /dev/null +++ b/smarts/core/glsl/map_values_shader.frag @@ -0,0 +1,40 @@ +#version 330 core +// Mask an image using another image +//#define SHADERTOY + +// Output color +out vec4 p3d_Color; + +uniform vec2 iResolution; +uniform sampler2D iChannel0; +uniform sampler2D iChannel1; + +uniform vec3 empty_color; +uniform float elapsed_sim_time; + + +void mainImage( out vec4 fragColor, in vec2 fragCoord ) +{ + vec2 rec_res = 1.0 / iResolution.xy; + vec2 p = fragCoord.xy * rec_res; + if (texture(iChannel0, p).x == 0) + { + fragColor = vec4(0.0, 0.0, 0.0, 1.0); + return; + } + + fragColor = texture(iChannel1, p); + + vec3 color = vec3(0.0, sin(elapsed_sim_time) * 0.5 + 1.0, 0.0); + //empty_color; + + if (fragColor.rgb == vec3(0.0, 0.0, 0.0)) { + fragColor = vec4(color, 1.0); + } +} + +#ifndef SHADERTOY +void main(){ + mainImage( p3d_Color, gl_FragCoord.xy ); +} +#endif \ No newline at end of file diff --git a/smarts/core/glsl/occlusion_shader.frag b/smarts/core/glsl/occlusion_shader.frag new file mode 100644 index 0000000000..04d21e191b --- /dev/null +++ b/smarts/core/glsl/occlusion_shader.frag @@ -0,0 +1,109 @@ +#version 330 core +// Visibility check + +// ----------------------------------------------- +#define STEP_LENGTH 0.2 +#define CENTER_COORD iResolution.xy * 0.5 +#define DEVICE_HEIGHT 0.8 +#define TOPOLOGY_SCALING_FACTOR 1.0 +#define CENTER vec2(0.5) +//#define SHADERTOY + +#ifdef SHADERTOY +float get_texture_max( vec2 uv ) { + return texture(iChannel0, uv).x; +} +#else +// Output color +out vec4 p3d_Color; + +uniform vec2 iResolution; +uniform float iElevation; +uniform sampler2D iChannel0; +uniform sampler2D iChannel1; + +float get_texture_max( vec2 uv ) { + return texture(iChannel0, uv).x * TOPOLOGY_SCALING_FACTOR + texture(iChannel1, uv).x; +} +#endif + +float cross_magnitude(vec2 lp1, vec2 lp2, vec2 p1){ + vec2 v1 = lp2 - lp1; + vec2 v2 = lp2 - p1; + + return v1.x * v2.y - v1.y * v2.x; +} + +float to_surface_face(vec2 rec_res, vec2 p, float aspect, float elevation) { + // Determine the minimum delta needed to determine the facing direction of the surface + float interpolate_delta = 0.7 * min(rec_res.x, rec_res.y); + vec2 direction = normalize(p - CENTER); + vec2 uv_offset_to_closer_p = direction * (interpolate_delta)*vec2(aspect,1.0); + + float surface_end_height = get_texture_max(p); + + // inspect fragments near mouse + + float surface_start_height = get_texture_max(p - uv_offset_to_closer_p); + + float viewer_height = get_texture_max(CENTER) + elevation; + vec2 line_start = vec2(distance(p - uv_offset_to_closer_p, CENTER), surface_start_height); + vec2 line_end = vec2(distance(p, CENTER), surface_end_height); + vec2 viewer_loc = vec2(0, viewer_height); + + + float cross_mag = cross_magnitude(line_start, line_end, viewer_loc); + return cross_mag; +} + +void mainImage( out vec4 fragColor, in vec2 fragCoord ) +{ + #ifdef SHADERTOY + float elevation = DEVICE_HEIGHT; + #else + float elevation = iElevation; + #endif + + vec2 rec_res = 1.0 / iResolution.xy; + vec2 p = fragCoord.xy * rec_res; + { + float aspect = iResolution.x/iResolution.y; + float cross_mag = to_surface_face(rec_res, p, aspect, elevation); + + if (-cross_mag <= 0.0){ + fragColor = vec4(0.0, 0.0, 0.0, 1.0); + return; + } + } + + vec2 offset = fragCoord.xy - CENTER_COORD; + vec2 offset_norm = normalize(offset); + float offset_dist = length(offset); + + float center_height = get_texture_max(vec2(0.5)) + elevation; + float target_height = get_texture_max(p); + + float target_slope = (target_height - center_height) / offset_dist; + + float total = STEP_LENGTH; + while (total < offset_dist - 0.01 ) { + vec2 intermediary_coord = (CENTER_COORD + offset_norm * total) * rec_res; + float intermediary_height = get_texture_max(intermediary_coord); + + if ( target_slope < (intermediary_height - center_height) / total){ + fragColor = vec4(0.0, 0.0, 0.0, 1.0); + return; + } + + total += STEP_LENGTH; + } + + + fragColor = vec4(1.0,1.0,1.0,1.0); +} + +#ifndef SHADERTOY +void main(){ + mainImage( p3d_Color, gl_FragCoord.xy ); +} +#endif \ No newline at end of file diff --git a/smarts/core/glsl/simplex_shader.frag b/smarts/core/glsl/simplex_shader.frag new file mode 100644 index 0000000000..0c00055778 --- /dev/null +++ b/smarts/core/glsl/simplex_shader.frag @@ -0,0 +1,109 @@ +#version 330 core +// The MIT License +// Copyright © 2013 Inigo Quilez +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// https://www.youtube.com/c/InigoQuilez +// https://iquilezles.org + + +// Simplex Noise (http://en.wikipedia.org/wiki/Simplex_noise), a type of gradient noise +// that uses N+1 vertices for random gradient interpolation instead of 2^N as in regular +// latice based Gradient Noise. + +// All noise functions here: +// +// https://www.shadertoy.com/playlist/fXlXzf&from=0&num=12 + + +// ----------------------------------------------- + +//#define SHADERTOY + +#define DENSITY_U 1.6 * 0.1 +#define DENSITY_V 1.2 * 0.1 + +#ifdef SHADERTOY +#define scale 1.0 + +#else +vec2 hash( vec2 p ) // replace this by something better +{ + p = vec2( dot(p,vec2(127.1,311.7)), dot(p,vec2(269.5,183.3)) ); + return -1.0 + 2.0*fract(sin(p)*43758.5453123); +} + +float noise( in vec2 p ) +{ + const float K1 = 0.366025404; // (sqrt(3)-1)/2; + const float K2 = 0.211324865; // (3-sqrt(3))/6; + + vec2 i = floor( p + (p.x+p.y)*K1 ); + vec2 a = p - i + (i.x+i.y)*K2; + float m = step(a.y,a.x); + vec2 o = vec2(m,1.0-m); + vec2 b = a - o + K2; + vec2 c = a - 1.0 + 2.0*K2; + vec3 h = max( 0.5-vec3(dot(a,a), dot(b,b), dot(c,c) ), 0.0 ); + vec3 n = h*h*h*h*vec3( dot(a,hash(i+0.0)), dot(b,hash(i+o)), dot(c,hash(i+1.0))); + return dot( n, vec3(70.0) ); +} + +float noise_with_octaves( in vec2 uv, in mat2 m ){ + float f = 0.0; + uv *= 5.0; + + f = 0.5000*noise( uv ); uv = m*uv; + f += 0.2500*noise( uv ); uv = m*uv; + f += 0.1250*noise( uv ); uv = m*uv; + f += 0.0625*noise( uv ); uv = m*uv; + return f; +} + +// Output color +out vec4 p3d_Color; + +// inputs +uniform vec2 iResolution; +uniform float iHeading; +uniform vec2 iTranslation; +uniform float scale; + +uniform sampler2D iChannel0; +#endif + +void mainImage( out vec4 fragColor, in vec2 fragCoord ) +{ + vec2 uv = ( fragCoord.xy - .5 * iResolution.xy ) / iResolution.y; + + #ifdef SHADERTOY + float rotation = iTime; + vec2 translation = iMouse.xy; + #else + float rotation = -iHeading; // Turn opposite the rotation of the object + vec2 translation = iTranslation * scale; + #endif + + float s = sin(rotation); + float c = cos(rotation); + uv = uv * mat2(c, s, -s, c) + translation / iResolution.y; + + float f = 0.0; + float inv_s = 1.0 / scale; + mat2 m = mat2( DENSITY_U, DENSITY_V, -DENSITY_V, DENSITY_U ) * inv_s; + f = noise_with_octaves(uv, m); + + vec2 p = fragCoord.xy * 1.0 / iResolution.xy; + if (texture(iChannel0, p).r < .2) { + f = f + 2.0 * abs(noise_with_octaves(uv, m * 2.0)); + } + //f = 0.5 + 0.5*f; + //f *= 0.3 + f; + + fragColor = vec4( f, f, f, f ); +} + +#ifndef SHADERTOY +void main(){ + mainImage(p3d_Color, gl_FragCoord.xy); +} +#endif \ No newline at end of file diff --git a/smarts/core/glsl/test_custom_shader_pass_shader.frag b/smarts/core/glsl/test_custom_shader_pass_shader.frag new file mode 100644 index 0000000000..653f2a3116 --- /dev/null +++ b/smarts/core/glsl/test_custom_shader_pass_shader.frag @@ -0,0 +1,126 @@ +#version 330 core +// This script is intended to test that all of the observation buffers work in association with +// the test script `test_renderers.py`. + +uniform int step_count; +uniform int steps_completed; +uniform int events_collisions; +uniform int events_off_road; +uniform int events_off_route; +uniform int events_on_shoulder; +uniform int events_wrong_way; +uniform int events_not_moving; +uniform int events_reached_goal; +uniform int events_reached_max_episode_steps; +uniform int events_agents_done_alive; +uniform int events_interest_done; +uniform int ego_vehicle_state_road_id; +uniform int ego_vehicle_state_lane_id; +uniform int ego_vehicle_state_lane_index; +uniform int under_this_vehicle_control; +uniform int vehicle_type; + +uniform float dt; +uniform float ego_vehicle_state_heading; +uniform float ego_vehicle_state_speed; +uniform float ego_vehicle_state_steering; +uniform float ego_vehicle_state_yaw_rate; +uniform float elapsed_sim_time; +uniform float distance_travelled; + +uniform vec3 ego_vehicle_state_position; +uniform vec3 ego_vehicle_state_bounding_box; +uniform vec3 ego_vehicle_state_lane_position; + +uniform int neighborhood_vehicle_states_road_id[10]; +uniform int neighborhood_vehicle_states_lane_id[10]; +uniform int neighborhood_vehicle_states_lane_index[10]; +uniform int neighborhood_vehicle_states_interest[10]; +uniform int waypoint_paths_lane_id[10]; +uniform int waypoint_paths_lane_index[10]; +uniform int road_waypoints_lanes_lane_id[10]; +uniform int road_waypoints_lanes_lane_index[10]; +uniform int via_data_near_via_points_lane_index[10]; +uniform int via_data_near_via_points_road_id[10]; +uniform int via_data_near_via_points_hit[10]; +uniform int lidar_point_cloud_hits[100]; +uniform int signals_light_state[10]; + +uniform float neighborhood_vehicle_states_heading[10]; +uniform float neighborhood_vehicle_states_speed[10]; +uniform float waypoint_paths_heading[10]; +uniform float waypoint_paths_lane_width[10]; +uniform float waypoint_paths_speed_limit[10]; +uniform float waypoint_paths_lane_offset[10]; +uniform float road_waypoints_lanes_heading[10]; +uniform float road_waypoints_lanes_width[10]; +uniform float road_waypoints_lanes_speed_limit[10]; +uniform float road_waypoints_lanes_lane_offset[10]; +uniform float via_data_near_via_points_required_speed[10]; +uniform float signals_last_changed[10]; + +uniform vec2 ego_vehicle_state_linear_velocity[10]; +uniform vec2 ego_vehicle_state_angular_velocity[10]; +uniform vec2 ego_vehicle_state_linear_acceleration[10]; +uniform vec2 ego_vehicle_state_angular_acceleration[10]; +uniform vec2 ego_vehicle_state_linear_jerk[10]; +uniform vec2 ego_vehicle_state_angular_jerk[10]; +uniform vec2 waypoint_paths_pos[10]; +uniform vec2 road_waypoints_lanes_pos[10]; +uniform vec2 via_data_near_via_points_position[10]; +uniform vec2 signals_stop_point[10]; + +uniform vec3 neighborhood_vehicle_states_position[10]; +uniform vec3 neighborhood_vehicle_states_bounding_box[10]; +uniform vec3 neighborhood_vehicle_states_lane_position[10]; +uniform vec3 lidar_point_cloud_points[100]; +uniform vec3 lidar_point_cloud_origin[100]; +uniform vec3 lidar_point_cloud_direction[100]; +// SIGNALS_CONTROLLED_LANES = "signals_controlled_lanes" + +// Output color +out vec4 p3d_Color; + +uniform vec2 iResolution; + + +void mainImage( out vec4 fragColor, in vec2 fragCoord ) +{ + vec2 rec_res = 1.0 / iResolution.xy; + vec2 p = fragCoord.xy * rec_res; + + fragColor = vec4(0.0, 0.0, 0.0, 0.0); +} + +void main(){ + int a = step_count + steps_completed + events_collisions + events_off_road + events_off_route + + events_on_shoulder + events_wrong_way + events_not_moving + + events_reached_goal + events_reached_max_episode_steps + + events_agents_done_alive + events_interest_done + ego_vehicle_state_road_id + + ego_vehicle_state_lane_id + ego_vehicle_state_lane_index + under_this_vehicle_control + vehicle_type; + float b = dt + ego_vehicle_state_heading + ego_vehicle_state_speed + + ego_vehicle_state_steering + ego_vehicle_state_yaw_rate + elapsed_sim_time + + distance_travelled; + vec3 c = ego_vehicle_state_position + ego_vehicle_state_bounding_box + ego_vehicle_state_lane_position; + int d = neighborhood_vehicle_states_road_id[0] + neighborhood_vehicle_states_lane_id[0] + + neighborhood_vehicle_states_lane_index[0] + neighborhood_vehicle_states_interest[0] + + waypoint_paths_lane_id[0] + waypoint_paths_lane_index[0] + road_waypoints_lanes_lane_id[0] + + road_waypoints_lanes_lane_index[0] + via_data_near_via_points_lane_index[0] + + via_data_near_via_points_road_id[0] + via_data_near_via_points_hit[0] + + lidar_point_cloud_hits[0] + signals_light_state[0]; + float e = neighborhood_vehicle_states_heading[0] + neighborhood_vehicle_states_speed[0] + + waypoint_paths_heading[0] + waypoint_paths_lane_width[0] + waypoint_paths_speed_limit[0] + + waypoint_paths_lane_offset[0] + road_waypoints_lanes_heading[0] + road_waypoints_lanes_width[0] + + road_waypoints_lanes_speed_limit[0] + road_waypoints_lanes_lane_offset[0] + + via_data_near_via_points_required_speed[0] + signals_last_changed[0]; + vec2 f = ego_vehicle_state_linear_velocity[0] + ego_vehicle_state_angular_velocity[0] + + ego_vehicle_state_linear_acceleration[0] + ego_vehicle_state_angular_acceleration[0] + + ego_vehicle_state_linear_jerk[0] + ego_vehicle_state_angular_jerk[0] + + waypoint_paths_pos[0] + road_waypoints_lanes_pos[0] + via_data_near_via_points_position[0] + + signals_stop_point[0]; + vec3 g = neighborhood_vehicle_states_position[0] + neighborhood_vehicle_states_bounding_box[0] + + neighborhood_vehicle_states_lane_position[0] + lidar_point_cloud_points[0] + + lidar_point_cloud_origin[0] + lidar_point_cloud_direction[0]; + + mainImage( p3d_Color, gl_FragCoord.xy ); +} \ No newline at end of file diff --git a/smarts/core/id_actor_capture_manager.py b/smarts/core/id_actor_capture_manager.py index 671271367a..1df9815df6 100644 --- a/smarts/core/id_actor_capture_manager.py +++ b/smarts/core/id_actor_capture_manager.py @@ -19,28 +19,29 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging -from typing import Dict, Optional, Set, Tuple +from typing import TYPE_CHECKING, Dict, Set, Tuple from smarts.core.actor_capture_manager import ActorCaptureManager from smarts.core.condition_state import ConditionState -from smarts.core.plan import Mission -from smarts.core.vehicle import Vehicle +from smarts.core.plan import NavigationMission from smarts.sstudio.sstypes import IdEntryTactic +if TYPE_CHECKING: + from smarts.core import scenario + from smarts.core.smarts import SMARTS + class IdActorCaptureManager(ActorCaptureManager): """The base for managers that handle transition of control of actors.""" def __init__(self) -> None: self._log = logging.getLogger(self.__class__.__name__) - self._actor_for_agent: Dict[str, Tuple[str, Mission]] = {} - - def step(self, sim): - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) + self._actor_for_agent: Dict[str, Tuple[str, NavigationMission]] = {} + def step(self, sim: SMARTS): if not ( sim.agent_manager.pending_agent_ids | sim.agent_manager.pending_social_agent_ids @@ -81,7 +82,7 @@ def step(self, sim): continue if not condition_result: continue - vehicle: Optional[Vehicle] = self._take_existing_vehicle( + vehicle = self._take_existing_vehicle( sim, actor_id, agent_id, @@ -94,16 +95,9 @@ def step(self, sim): for actor_id in used_actors: del self._actor_for_agent[actor_id] - def reset(self, scenario, sim): - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) - from smarts.core.scenario import Scenario - - assert isinstance(scenario, Scenario) - + def reset(self, scenario, sim: SMARTS): self._actor_for_agent.clear() - missions: Dict[str, Mission] = scenario.missions + missions: Dict[str, NavigationMission] = scenario.missions cancelled_agents = set() for agent_id, mission in missions.items(): if mission is None: diff --git a/smarts/core/lanepoints.py b/smarts/core/lanepoints.py index 67f05e20d6..13e9baf976 100644 --- a/smarts/core/lanepoints.py +++ b/smarts/core/lanepoints.py @@ -25,14 +25,16 @@ from collections import defaultdict from dataclasses import dataclass from functools import lru_cache -from typing import List, NamedTuple, Sequence, Tuple +from typing import TYPE_CHECKING, List, NamedTuple, Sequence, Tuple import numpy as np from smarts.core.coordinates import Heading, Point, Pose -from smarts.core.road_map import RoadMap from smarts.core.utils.core_math import fast_quaternion_from_angle, vec_to_radians +if TYPE_CHECKING: + from smarts.core.road_map import RoadMap + @dataclass(frozen=True) class LanePoint: diff --git a/smarts/core/lidar.py b/smarts/core/lidar.py index 27258d8af3..21c869f357 100644 --- a/smarts/core/lidar.py +++ b/smarts/core/lidar.py @@ -17,17 +17,20 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import itertools import random -from typing import List, Sequence, Tuple +from typing import TYPE_CHECKING, List, Sequence, Tuple import numpy as np import psutil -from .lidar_sensor_params import SensorParams from .utils import pybullet from .utils.core_math import batches, rotate_quat -from .utils.pybullet import bullet_client as bc + +if TYPE_CHECKING: + from .lidar_sensor_params import SensorParams class Lidar: @@ -96,9 +99,9 @@ def _compute_rays(self): yaws = -1 * self._sensor_params.laser_angles rolls = np.arange(n_rays) * self._sensor_params.angle_resolution + origin = np.array([0, 0, 0]) for yaw, roll in itertools.product(yaws, rolls): rot = pybullet.getQuaternionFromEuler((roll, 0, yaw)) - origin = np.array([0, 0, 0]) direction = rotate_quat( np.asarray(rot, dtype=float), diff --git a/smarts/core/local_agent.py b/smarts/core/local_agent.py index f0d10f32ee..629d3f99fa 100644 --- a/smarts/core/local_agent.py +++ b/smarts/core/local_agent.py @@ -17,18 +17,24 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + from concurrent import futures +from typing import TYPE_CHECKING from smarts.core.buffer_agent import BufferAgent -from smarts.zoo.agent_spec import AgentSpec + +if TYPE_CHECKING: + from smarts.core.agent import Agent + from smarts.zoo.agent_spec import AgentSpec class LocalAgent(BufferAgent): """A local implementation of a buffer agent.""" def __init__(self): - self._agent = None - self._agent_spec = None + self._agent: Agent = None + self._agent_spec: AgentSpec = None def act(self, obs): """Call the agent's act function asynchronously and return a Future.""" diff --git a/smarts/core/local_traffic_provider.py b/smarts/core/local_traffic_provider.py index ae014be94d..f8d86308af 100644 --- a/smarts/core/local_traffic_provider.py +++ b/smarts/core/local_traffic_provider.py @@ -17,6 +17,7 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations import logging import math @@ -32,23 +33,21 @@ import numpy as np from shapely.affinity import rotate as shapely_rotate -from shapely.geometry import Polygon from shapely.geometry import box as shapely_box from smarts.core.utils.core_logging import timeit from .actor import ActorRole, ActorState -from .controllers import ActionSpaceType from .coordinates import Dimensions, Heading, Point, Pose, RefLinePoint from .provider import Provider, ProviderManager, ProviderRecoveryFlags, ProviderState from .road_map import RoadMap from .route_cache import RouteWithCache -from .scenario import Scenario from .signals import SignalLightState, SignalState from .traffic_provider import TrafficProvider from .utils.core_math import ( min_angles_difference_signed, radians_to_vec, + safe_division, vec_to_radians, ) from .utils.kinematics import ( @@ -62,18 +61,11 @@ if TYPE_CHECKING: from shapely.geometry import Polygon + import smarts.core.scenario from smarts.core.controllers import ActionSpaceType - from smarts.core.scenario import Scenario - -MAX_IMPATIENCE = 3.0 -def _safe_division(n: float, d: float, default=math.inf): - """This method uses a short circuit form where `and` converts right side to true|false (as 1|0) in which cases are: - True and # == # - False and NaN == False - """ - return d and n / d or default +MAX_IMPATIENCE = 3.0 class LocalTrafficProvider(TrafficProvider): @@ -149,7 +141,7 @@ def _load_traffic_flows(self, traffic_spec: str): f"vehPerHour value is {freq}<=0 vehicles may not be emitted!!!" ) freq = 0 - flow["emit_period"] = _safe_division(3600.0, freq) + flow["emit_period"] = safe_division(3600.0, freq) elif "period" in flow: period = float(flow["period"]) assert period > 0.0 @@ -222,7 +214,7 @@ def _all_states(self) -> List[VehicleState]: def _provider_state(self) -> ProviderState: return ProviderState(actors=self._my_actor_states) - def setup(self, scenario: Scenario) -> ProviderState: + def setup(self, scenario: smarts.core.scenario.Scenario) -> ProviderState: assert self._sim() is not None self._scenario = scenario self.road_map = scenario.road_map @@ -387,7 +379,7 @@ def stop_managing(self, actor_id: str): def reserve_traffic_location_for_vehicle( self, vehicle_id: str, - reserved_location: Polygon, + reserved_location, ): self._reserved_areas[vehicle_id] = reserved_location @@ -841,9 +833,9 @@ def _angle_scale(self, to_index: int, theta: float = math.pi / 6) -> float: # we need to correct for not going straight across. # other things being equal, we target ~30 degrees (sin(30)=.5) on average. if abs(self.radius) > 1e5 or self.radius == 0: - return _safe_division(1.0, math.sin(theta), 1e6) + return safe_division(1.0, math.sin(theta), 1e6) # here we correct for the local road curvature (which affects how far we must travel)... - T = _safe_division(self.radius, self.width, 1e6) + T = safe_division(self.radius, self.width, 1e6) # XXX: This cannot be an assertion because it could happen for map related reasons. if abs(T) <= 1.0: logging.debug( @@ -862,7 +854,7 @@ def _angle_scale(self, to_index: int, theta: float = math.pi / 6) -> float: + 0.5 - se * math.cos( - _safe_division(1, (math.tan(theta) * (T - 1)), default=0) + safe_division(1, (math.tan(theta) * (T - 1)), default=0) ) ) ) @@ -873,9 +865,7 @@ def _angle_scale(self, to_index: int, theta: float = math.pi / 6) -> float: se + 0.5 - se - * math.cos( - _safe_division(1, (math.tan(theta) * (T + 1)), default=0) - ) + * math.cos(safe_division(1, (math.tan(theta) * (T + 1)), default=0)) ) ) @@ -1026,7 +1016,7 @@ def _compute_lane_window(self, lane: RoadMap.Lane): rt_ln = RoadMap.Route.RouteLane(lane, self._route_ind) path_len = self._route.distance_from(rt_ln) or lane.length path_len -= my_offset - lane_time_left = _safe_division(path_len, self.speed) + lane_time_left = safe_division(path_len, self.speed) half_len = 0.5 * self._state.dimensions.length front_bumper = my_offset + half_len @@ -1115,7 +1105,7 @@ def _should_cutin(self, lw: _LaneWindow) -> bool: self.speed, self._max_decel ): return False - min_gap = _safe_division( + min_gap = safe_division( self._target_cutin_gap, self._aggressiveness, default=1e5 ) max_gap = self._target_cutin_gap + 2 @@ -1265,7 +1255,7 @@ def _get_radius(lane: RoadMap.Lane) -> float: if l != self._lane and abs(my_radius) < 1e5: l_radius = _get_radius(l) if abs(l_radius) < 1e5: - ratio = _safe_division(l_radius, my_radius, default=0) + ratio = safe_division(l_radius, my_radius, default=0) if ratio < 0: ratio = 1.0 self._lane_speed[l.index] = (ratio * self.speed, ratio * self.acceleration) @@ -1370,10 +1360,10 @@ def predict_crash_in(self, veh_id: str, bumper: int) -> float: final_range = window[-1].dist # the exponent here was determined by trial and error - if range_del < 0 and abs(bearing_del) < _safe_division( + if range_del < 0 and abs(bearing_del) < safe_division( math.pi, final_range**1.4 ): - return _safe_division(-final_range, range_del) + return safe_division(-final_range, range_del) return math.inf def purge_unseen(self, seen: Set[str]): @@ -1773,10 +1763,10 @@ def _compute_acceleration(self, dt: float) -> float: time_cush = max( min( self._target_lane_win.ttc, - _safe_division(self._target_lane_win.gap, speed_denom), + safe_division(self._target_lane_win.gap, speed_denom), self._target_lane_win.time_left, self._lane_win.ttc, - _safe_division(self._lane_win.gap, speed_denom), + safe_division(self._lane_win.gap, speed_denom), 2 * self._lane_win.time_left, ), 1e-13, @@ -1788,16 +1778,14 @@ def _compute_acceleration(self, dt: float) -> float: and time_cush < min_time_cush ): if self.speed > 0: - severity = 4 * _safe_division( - (min_time_cush - time_cush), min_time_cush - ) + severity = 4 * safe_division((min_time_cush - time_cush), min_time_cush) return -emergency_decl * np.clip(severity, 0, 1.0) return 0 space_cush = max(min(self._target_lane_win.gap, self._lane_win.gap), 1e-13) if space_cush < self._min_space_cush - self._min_space_cush * self._impatience: if self.speed > 0: - severity = 4 * _safe_division( + severity = 4 * safe_division( (self._min_space_cush - space_cush), self._min_space_cush ) return -emergency_decl * np.clip(severity, 0, 1.0) diff --git a/smarts/core/masks.py b/smarts/core/masks.py index 86a1121c3c..a573da7982 100644 --- a/smarts/core/masks.py +++ b/smarts/core/masks.py @@ -20,6 +20,7 @@ class RenderMasks: """Rendering mask flags""" + NONE = 0x00 OCCUPANCY_HIDE = 0x01 RGB_HIDE = 0x02 DRIVABLE_AREA_HIDE = 0x04 diff --git a/smarts/core/observations.py b/smarts/core/observations.py index bf38326327..e00759cea2 100644 --- a/smarts/core/observations.py +++ b/smarts/core/observations.py @@ -19,16 +19,19 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -from typing import Dict, List, NamedTuple, Optional, Tuple +from __future__ import annotations + +from typing import TYPE_CHECKING, Dict, List, NamedTuple, Optional, Tuple import numpy as np -from smarts.core.coordinates import Dimensions, Heading, Point, RefLinePoint -from smarts.core.plan import Mission -from smarts.core.road_map import Waypoint -from smarts.core.signals import SignalLightState +from smarts.core.utils.cache import cache -from .events import Events +if TYPE_CHECKING: + from smarts.core import plan, signals + from smarts.core.coordinates import Dimensions, Heading, Point, RefLinePoint + from smarts.core.events import Events + from smarts.core.road_map import Waypoint class VehicleObservation(NamedTuple): @@ -62,8 +65,8 @@ class EgoVehicleObservation(NamedTuple): id: str """Vehicle identifier.""" - position: np.ndarray - """Center coordinate of the vehicle bounding box's bottom plane. `shape=(3,)`. `dtype=np.float64`.""" + position: Tuple[float, float, float] + """Center coordinate of the vehicle bounding box's bottom plane.""" bounding_box: Dimensions """Bounding box describing the length, width, and height, of the vehicle.""" heading: Heading @@ -80,20 +83,20 @@ class EgoVehicleObservation(NamedTuple): """Identifier for the lane nearest to this vehicle.""" lane_index: int """Index of the nearest lane on the road nearest to this vehicle. Right most lane has index 0 and index increments to the left.""" - mission: Mission + mission: plan.NavigationMission """Vehicle's desired destination.""" - linear_velocity: np.ndarray - """Velocity of vehicle along the global coordinate axes. Units=m/s. A numpy array of shape=(3,) and dtype=np.float64.""" - angular_velocity: np.ndarray - """Velocity of vehicle-heading rotation about the z-axis. Equivalent vector representation of yaw_rate. Units=rad/s. A numpy array of shape=(3,) and dtype=np.float64.""" - linear_acceleration: Optional[np.ndarray] - """Acceleration of vehicle along the global coordinate axes. Units=m/s^2. A numpy array of shape=(3,). dtype=np.float64. Requires accelerometer sensor.""" - angular_acceleration: Optional[np.ndarray] - """Acceleration of vehicle-heading rotation about the z-axis. Units=rad/s^2. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor.""" - linear_jerk: Optional[np.ndarray] - """Jerk of vehicle along the global coordinate axes. Units=m/s^3. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor.""" - angular_jerk: Optional[np.ndarray] - """Jerk of vehicle-heading rotation about the z-axis. Units=rad/s^3. A numpy array of shape=(3,) and dtype=np.float64. Requires accelerometer sensor.""" + linear_velocity: Tuple[float, float, float] + """Velocity of vehicle along the global coordinate axes. Units=m/s.""" + angular_velocity: Tuple[float, float, float] + """Velocity of vehicle-heading rotation about the z-axis. Equivalent vector representation of yaw_rate. Units=rad/s.""" + linear_acceleration: Optional[Tuple[float, float, float]] + """Acceleration of vehicle along the global coordinate axes. Units=m/s^2. Requires accelerometer sensor.""" + angular_acceleration: Optional[Tuple[float, float, float]] + """Acceleration of vehicle-heading rotation about the z-axis. Units=rad/s^2. Requires accelerometer sensor.""" + linear_jerk: Optional[Tuple[float, float, float]] + """Jerk of vehicle along the global coordinate axes. Units=m/s^3. Requires accelerometer sensor.""" + angular_jerk: Optional[Tuple[float, float, float]] + """Jerk of vehicle-heading rotation about the z-axis. Units=rad/s^3. Requires accelerometer sensor.""" lane_position: Optional[RefLinePoint] = None """(s,t,h) coordinates within the lane, where s is the longitudinal offset along the lane, t is the lateral displacement from the lane center, and h (not yet supported) is the vertical displacement from the lane surface. See the Reference Line coordinate system in OpenDRIVE here: https://www.asam.net/index.php?eID=dumpFile&t=f&f=4089&token=deea5d707e2d0edeeb4fccd544a973de4bc46a09#_coordinate_systems """ @@ -105,6 +108,9 @@ class RoadWaypoints(NamedTuple): lanes: Dict[str, List[List[Waypoint]]] """Mapping of road ids to their lane waypoints.""" + def __hash__(self) -> int: + return hash(tuple((k, len(v)) for k, v in self.lanes.items())) + class GridMapMetadata(NamedTuple): """Map grid metadata.""" @@ -129,6 +135,9 @@ class TopDownRGB(NamedTuple): data: np.ndarray """A RGB image with the ego vehicle at the center.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + class OccupancyGridMap(NamedTuple): """Occupancy map.""" @@ -140,6 +149,21 @@ class OccupancyGridMap(NamedTuple): See https://en.wikipedia.org/wiki/Occupancy_grid_mapping.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + + +class OcclusionRender(NamedTuple): + """Occlusion map.""" + + metadata: GridMapMetadata + """Map metadata.""" + data: np.ndarray + """A map showing what is visible from the ego vehicle""" + + def __hash__(self) -> int: + return self.metadata.__hash__() + class DrivableAreaGridMap(NamedTuple): """Drivable area map.""" @@ -149,6 +173,21 @@ class DrivableAreaGridMap(NamedTuple): data: np.ndarray """A grid map that shows the static drivable area around the ego vehicle.""" + def __hash__(self) -> int: + return self.metadata.__hash__() + + +class CustomRenderData(NamedTuple): + """Describes information about a custom render.""" + + metadata: GridMapMetadata + """Render metadata.""" + data: np.ndarray + """The image data from the render.""" + + def __hash__(self) -> int: + return self.metadata.__hash__() + class ViaPoint(NamedTuple): """'Collectibles' that can be placed within the simulation.""" @@ -161,26 +200,31 @@ class ViaPoint(NamedTuple): """Road id this collectible is associated with.""" required_speed: float """Approximate speed required to collect this collectible.""" + hit: bool + """If this via point was hit in the last step.""" class Vias(NamedTuple): """Listing of nearby collectible ViaPoints and ViaPoints collected in the last step.""" - near_via_points: List[ViaPoint] + near_via_points: Tuple[ViaPoint] """Ordered list of nearby points that have not been hit.""" - hit_via_points: List[ViaPoint] - """List of points that were hit in the previous step.""" + + @property + def hit_via_points(self) -> Tuple[ViaPoint]: + """List of points that were hit in the previous step.""" + return tuple(vp for vp in self.near_via_points if vp.hit) class SignalObservation(NamedTuple): """Describes an observation of a traffic signal (light) on this time-step.""" - state: SignalLightState + state: signals.SignalLightState """The state of the traffic signal.""" stop_point: Point """The stopping point for traffic controlled by the signal, i.e., the point where actors should stop when the signal is in a stop state.""" - controlled_lanes: List[str] + controlled_lanes: Tuple[str] """If known, the lane_ids of all lanes controlled-by this signal. May be empty if this is not easy to determine.""" last_changed: Optional[float] @@ -204,7 +248,7 @@ class Observation(NamedTuple): """Ego vehicle status.""" under_this_agent_control: bool """Whether this agent currently has control of the vehicle.""" - neighborhood_vehicle_states: Optional[List[VehicleObservation]] + neighborhood_vehicle_states: Optional[Tuple[VehicleObservation]] """List of neighborhood vehicle states.""" waypoint_paths: Optional[List[List[Waypoint]]] """Dynamic evenly-spaced points on the road ahead of the vehicle, showing potential routes ahead.""" @@ -226,5 +270,31 @@ class Observation(NamedTuple): """Occupancy map.""" top_down_rgb: Optional[TopDownRGB] = None """RGB camera observation.""" - signals: Optional[List[SignalObservation]] = None + signals: Optional[Tuple[SignalObservation]] = None """List of nearby traffic signal (light) states on this time-step.""" + occlusion_map: Optional[OcclusionRender] = None + """Observable area map.""" + custom_renders: Tuple[CustomRenderData, ...] = tuple() + """Custom renders.""" + + def __hash__(self): + return hash( + ( + self.dt, + self.step_count, + self.elapsed_sim_time, + self.events, + self.ego_vehicle_state, + # self.waypoint_paths, # likely redundant + self.neighborhood_vehicle_states, + self.distance_travelled, + self.road_waypoints, + self.via_data, + self.drivable_area_grid_map, + self.occupancy_grid_map, + self.top_down_rgb, + self.signals, + self.occlusion_map, + self.custom_renders, + ) + ) diff --git a/smarts/core/plan.py b/smarts/core/plan.py index a9ac288df0..3d60e623f0 100644 --- a/smarts/core/plan.py +++ b/smarts/core/plan.py @@ -67,6 +67,19 @@ def from_pose(cls, pose: Pose): from_front_bumper=False, ) + def __hash__(self): + hash_ = getattr(self, "hash", None) + if not hash_: + hash_ = hash( + ( + tuple(self.position), + self.heading, + self.from_front_bumper, + ) + ) + object.__setattr__(self, "hash", hash_) + return hash_ + @dataclass(frozen=True, unsafe_hash=True) class Goal: @@ -200,7 +213,7 @@ class VehicleSpec: @dataclass(frozen=True) -class Mission: +class NavigationMission: """A navigation mission describing a desired trip.""" # XXX: Note that this Mission differs from sstudio.sstypes.Mission in that @@ -231,9 +244,9 @@ def is_complete(self, vehicle_state, distance_travelled: float) -> bool: @staticmethod def endless_mission( start_pose: Pose, - ) -> Mission: + ) -> NavigationMission: """Generate an endless mission.""" - return Mission( + return NavigationMission( start=Start(start_pose.as_position2d(), start_pose.heading), goal=EndlessGoal(), entry_tactic=None, @@ -244,7 +257,7 @@ def random_endless_mission( road_map: RoadMap, min_range_along_lane: float = 0.3, max_range_along_lane: float = 0.9, - ) -> Mission: + ) -> NavigationMission: """A mission that starts from a random location and continues indefinitely.""" assert min_range_along_lane > 0 # Need to start further than beginning of lane assert max_range_along_lane < 1 # Cannot start past end of lane @@ -260,7 +273,7 @@ def random_endless_mission( offset *= n_lane.length coord = n_lane.from_lane_coord(RefLinePoint(offset)) target_pose = n_lane.center_pose_at_point(coord) - return Mission.endless_mission(start_pose=target_pose) + return NavigationMission.endless_mission(start_pose=target_pose) def __post_init__(self): if self.entry_tactic is not None and self.entry_tactic.start_time != MISSING: @@ -270,7 +283,7 @@ def __post_init__(self): @dataclass(frozen=True) -class LapMission(Mission): +class LapMission(NavigationMission): """A mission requiring a number of laps through the goal.""" num_laps: Optional[int] = None # None means infinite # of laps @@ -299,7 +312,7 @@ class PlanFrame: """Describes a plan that is serializable.""" road_ids: List[str] - mission: Optional[Mission] + mission: Optional[NavigationMission] @classmethod def empty(cls: Type[PlanFrame]): @@ -313,7 +326,7 @@ class Plan: def __init__( self, road_map: RoadMap, - mission: Optional[Mission] = None, + mission: Optional[NavigationMission] = None, find_route: bool = True, ): self._road_map = road_map @@ -333,7 +346,7 @@ def route(self, route: RoadMap.Route): self._route = route @property - def mission(self) -> Optional[Mission]: + def mission(self) -> Optional[NavigationMission]: """The mission this plan is meant to fulfill.""" # XXX: This currently can be `None` return self._mission @@ -345,7 +358,7 @@ def road_map(self) -> RoadMap: def create_route( self, - mission: Mission, + mission: NavigationMission, start_lane_radius: Optional[float] = None, end_lane_radius: Optional[float] = None, ): @@ -366,7 +379,9 @@ def create_route( assert not self._route or not len( self._route.road_ids ), "Already called create_route()." - self._mission = mission or Mission.random_endless_mission(self._road_map) + self._mission = mission or NavigationMission.random_endless_mission( + self._road_map + ) if not self._mission.requires_route: self._route = self._road_map.empty_route() @@ -380,7 +395,7 @@ def create_route( radius=start_lane_radius, ) if not start_lanes: - self._mission = Mission.endless_mission(Pose.origin()) + self._mission = NavigationMission.endless_mission(Pose.origin()) raise PlanningError("Starting lane not found. Route must start in a lane.") via_roads = [self._road_map.road_by_id(via) for via in self._mission.route_vias] @@ -407,7 +422,7 @@ def create_route( break if len(self._route.roads) == 0: - self._mission = Mission.endless_mission(Pose.origin()) + self._mission = NavigationMission.endless_mission(Pose.origin()) start_road_ids = [start_lane.road.road_id for start_lane, _ in start_lanes] raise PlanningError( "Unable to find a route between start={} and end={}. If either of " diff --git a/smarts/core/provider.py b/smarts/core/provider.py index 24aeaf9e5a..793e81bd62 100644 --- a/smarts/core/provider.py +++ b/smarts/core/provider.py @@ -17,14 +17,18 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging from dataclasses import dataclass, field from enum import IntFlag -from typing import Iterable, List, Optional, Set, Tuple +from typing import TYPE_CHECKING, Iterable, List, Optional, Set, Tuple from .actor import ActorState from .controllers import ActionSpaceType -from .scenario import Scenario + +if TYPE_CHECKING: + import smarts.core.scenario class ProviderRecoveryFlags(IntFlag): @@ -49,7 +53,7 @@ class ProviderState: actors: List[ActorState] = field(default_factory=list) dt: Optional[float] = None # most Providers can leave this blank - def merge(self, other: "ProviderState"): + def merge(self, other: ProviderState): """Merge state with another provider's state.""" our_actors = {a.actor_id for a in self.actors} other_actors = {a.actor_id for a in other.actors} @@ -71,7 +75,7 @@ def merge(self, other: "ProviderState"): self.dt = max(self.dt, other.dt, key=lambda x: x if x else 0) - def filter(self, actor_ids): + def filter(self, actor_ids: Set[str]): """Filter actor states down to the given actors.""" provider_actor_ids = [a.actor_id for a in self.actors] for a_id in actor_ids: @@ -119,8 +123,8 @@ class ProviderManager: # other Providers that are willing to accept new actors could watch for this. def provider_releases_actor( - self, current_provider: Optional["Provider"], state: ActorState - ) -> Optional["Provider"]: + self, current_provider: Optional[Provider], state: ActorState + ) -> Optional[Provider]: """The current provider gives up control over the specified actor. The manager finds a new Provider for the actor from among the Providers managed by this ProviderManager. If no provider accepts the actor the actor is removed from all providers. @@ -145,8 +149,8 @@ def provider_releases_actor( return new_provider def provider_relinquishing_actor( - self, current_provider: Optional["Provider"], state: ActorState - ) -> Tuple[Optional["Provider"], "ActorProviderTransition"]: + self, current_provider: Optional[Provider], state: ActorState + ) -> Tuple[Optional[Provider], ActorProviderTransition]: """Find a new Provider for an actor from among the Providers managed by this ProviderManager. @@ -155,13 +159,13 @@ def provider_relinquishing_actor( """ raise NotImplementedError - def provider_removing_actor(self, provider: Optional["Provider"], actor_id: str): + def provider_removing_actor(self, provider: Optional[Provider], actor_id: str): """Called by a Provider when it is removing an actor from the simulation. It means that the Provider is indicating that the actor no longer exists. This was added for convenience, but it isn't always necessary to be called.""" raise NotImplementedError - def provider_for_actor(self, actor_id: str) -> Optional["Provider"]: + def provider_for_actor(self, actor_id: str) -> Optional[Provider]: """Find the provider that currently manages the given actor. Args: @@ -174,9 +178,9 @@ def provider_for_actor(self, actor_id: str) -> Optional["Provider"]: def transition_to_provider( self, - new_provider: "Provider", - actor_provider_transition: "ActorProviderTransition", - ) -> Optional["Provider"]: + new_provider: Provider, + actor_provider_transition: ActorProviderTransition, + ) -> Optional[Provider]: """Passes a released actor to a new provider. This depends on `provider_relinquishing_actor`. Args: @@ -241,7 +245,7 @@ def actions(self) -> Set[ActionSpaceType]: """The action spaces of the provider.""" raise NotImplementedError - def setup(self, scenario: Scenario) -> ProviderState: + def setup(self, scenario) -> ProviderState: """Initialize the provider with a scenario.""" raise NotImplementedError @@ -268,7 +272,7 @@ def can_accept_actor(self, state: ActorState) -> bool: return False def add_actor( - self, provider_actor: ActorState, from_provider: Optional["Provider"] = None + self, provider_actor: ActorState, from_provider: Optional[Provider] = None ): """Management of the actor with state is being assigned (or transferred if from_provider is not None) to this Provider. @@ -284,7 +288,10 @@ def teardown(self): raise NotImplementedError def recover( - self, scenario, elapsed_sim_time: float, error: Optional[Exception] = None + self, + scenario: smarts.core.scenario.Scenario, + elapsed_sim_time: float, + error: Optional[Exception] = None, ) -> Tuple[ProviderState, bool]: """Attempt to reconnect the provider if an error or disconnection occurred. Implementations may choose to re-raise the passed in exception. @@ -340,7 +347,7 @@ def remove_actor(self, actor_id: str): # can be overridden to do more cleanup as necessary @classmethod - def provider_id(cls): + def provider_id(cls) -> str: """The identifying name of the provider.""" return cls.__name__ diff --git a/smarts/core/renderer_base.py b/smarts/core/renderer_base.py index 5fd88094ed..24c22db7f2 100644 --- a/smarts/core/renderer_base.py +++ b/smarts/core/renderer_base.py @@ -23,14 +23,19 @@ from __future__ import annotations import logging +from abc import ABCMeta, abstractmethod from dataclasses import dataclass from enum import IntEnum -from typing import Tuple +from pathlib import Path +from typing import TYPE_CHECKING, Collection, Tuple, Union import numpy as np from .coordinates import Pose +if TYPE_CHECKING: + from smarts.core.shader_buffer import BufferID + class DEBUG_MODE(IntEnum): """The rendering debug information level.""" @@ -42,12 +47,17 @@ class DEBUG_MODE(IntEnum): ERROR = 5 +class RendererNotSetUpWarning(UserWarning): + """This occurs if a renderer is used without being set up.""" + + @dataclass -class OffscreenCamera: +class OffscreenCamera(metaclass=ABCMeta): """A camera used for rendering images to a graphics buffer.""" renderer: RendererBase + @abstractmethod def wait_for_ram_image(self, img_format: str, retries=100): """Attempt to acquire a graphics buffer.""" # Rarely, we see dropped frames where an image is not available @@ -60,7 +70,8 @@ def wait_for_ram_image(self, img_format: str, retries=100): # we are fairly certain we have an image in ram to return to the user raise NotImplementedError - def update(self, pose: Pose, height: float): + @abstractmethod + def update(self, *args, **kwargs): """Update the location of the camera. Args: pose: @@ -71,26 +82,87 @@ def update(self, pose: Pose, height: float): raise NotImplementedError @property + @abstractmethod def image_dimensions(self) -> Tuple[int, int]: """The dimensions of the output camera image.""" raise NotImplementedError @property + @abstractmethod def position(self) -> Tuple[float, float, float]: """The position of the camera.""" raise NotImplementedError @property + @abstractmethod def heading(self) -> float: """The heading of this camera.""" raise NotImplementedError + @abstractmethod def teardown(self): """Clean up internal resources.""" raise NotImplementedError -class RendererBase: +@dataclass(frozen=True) +class ShaderStepDependencyBase: + """The base for shader dependencies.""" + + +@dataclass(frozen=True) +class ShaderStepVariableDependency(ShaderStepDependencyBase): + """The base for shader dependencies.""" + + value: Union[int, float, bool, np.ndarray, list, tuple] + script_variable_name: str + + def __post_init__(self): + assert self.value, f"`{self.script_variable_name=}` cannot be None or empty." + assert self.script_variable_name + assert ( + 0 < len(self.value) < 5 + if isinstance(self.value, (np.ndarray, list, tuple)) + else True + ) + + +@dataclass(frozen=True) +class ShaderStepBufferDependency(ShaderStepDependencyBase): + """The base for shader dependencies.""" + + buffer_id: BufferID + script_uniform_name: str + + def __post_init__(self): + assert self.buffer_id, f"`{self.script_uniform_name=}` cannot be None or empty." + assert self.script_uniform_name + + +@dataclass(frozen=True) +class ShaderStepCameraDependency(ShaderStepDependencyBase): + """Forwards the texture from a given camera to the""" + + camera_id: str + script_variable_name: str + + def __post_init__(self): + assert self.script_variable_name, "Variable name cannot be empty." + assert ( + self.camera_id + ), f"Camera id for {self.script_variable_name} cannot be None or empty." + + +@dataclass +class ShaderStep(OffscreenCamera, metaclass=ABCMeta): + """A camera used for rendering images using a shader and a full-screen quad.""" + + shader_file: str + camera_dependencies: Collection[OffscreenCamera] + buffer_dependencies: Collection[ShaderStepBufferDependency] + + +class RendererBase(metaclass=ABCMeta): """The base class for rendering Returns: @@ -98,72 +170,89 @@ class RendererBase: """ @property + @abstractmethod def id(self): """The id of the simulation rendered.""" raise NotImplementedError @property + @abstractmethod def is_setup(self) -> bool: """If the renderer has been fully initialized.""" raise NotImplementedError @property + @abstractmethod def log(self) -> logging.Logger: """The rendering logger.""" raise NotImplementedError + @abstractmethod def remove_buffer(self, buffer): """Remove the rendering buffer.""" raise NotImplementedError + @abstractmethod def setup(self, scenario): """Initialize this renderer.""" raise NotImplementedError + @abstractmethod def render(self): """Render the scene graph of the simulation.""" raise NotImplementedError + @abstractmethod def reset(self): """Reset the render back to initialized state.""" raise NotImplementedError + @abstractmethod def step(self): """provided for non-SMARTS uses; normally not used by SMARTS.""" raise NotImplementedError + @abstractmethod def sync(self, sim_frame): """Update the current state of the vehicles within the renderer.""" raise NotImplementedError + @abstractmethod def teardown(self): """Clean up internal resources.""" raise NotImplementedError + @abstractmethod def destroy(self): """Destroy the renderer. Cleans up all remaining renderer resources.""" raise NotImplementedError + @abstractmethod def create_vehicle_node(self, glb_model: str, vid: str, color, pose: Pose): """Create a vehicle node.""" raise NotImplementedError + @abstractmethod def begin_rendering_vehicle(self, vid: str, is_agent: bool): """Add the vehicle node to the scene graph""" raise NotImplementedError + @abstractmethod def update_vehicle_node(self, vid: str, pose: Pose): """Move the specified vehicle node.""" raise NotImplementedError + @abstractmethod def remove_vehicle_node(self, vid: str): """Remove a vehicle node""" raise NotImplementedError + @abstractmethod def camera_for_id(self, camera_id) -> OffscreenCamera: """Get a camera by its id.""" raise NotImplementedError + @abstractmethod def build_offscreen_camera( self, name: str, @@ -171,6 +260,21 @@ def build_offscreen_camera( width: int, height: int, resolution: float, - ) -> str: + ) -> None: """Generates a new off-screen camera.""" raise NotImplementedError + + @abstractmethod + def build_shader_step( + self, + name: str, + fshader_path: Union[str, Path], + dependencies: Collection[ + Union[ShaderStepCameraDependency, ShaderStepVariableDependency] + ], + priority: int, + height: int, + width: int, + ) -> None: + """Generates a new shader camera.""" + raise NotImplementedError diff --git a/smarts/core/road_map.py b/smarts/core/road_map.py index 53022e0bbc..6fed00f569 100644 --- a/smarts/core/road_map.py +++ b/smarts/core/road_map.py @@ -36,6 +36,7 @@ from smarts.core.coordinates import BoundingBox, Heading, Point, Pose, RefLinePoint from smarts.core.utils.core_math import ( fast_quaternion_from_angle, + lerp, min_angles_difference_signed, signed_dist_to_line, vec_to_radians, @@ -808,6 +809,14 @@ class Waypoint: lane_offset: float """Longitudinal distance along lane center-line of this waypoint.""" + def __post_init__(self): + if isinstance(self.lane_width, (np.floating)): + object.__setattr__(self, "lane_width", float(self.lane_width)) + if isinstance(self.speed_limit, (np.floating)): + object.__setattr__(self, "speed_limit", float(self.speed_limit)) + if isinstance(self.lane_offset, (np.floating)): + object.__setattr__(self, "lane_offset", float(self.lane_offset)) + def __eq__(self, other) -> bool: if not isinstance(other, Waypoint): return False @@ -847,6 +856,11 @@ def relative_heading(self, h: Heading) -> Heading: ) return self.heading.relative_to(h) + @property + def position(self): + """The position of the waypoint.""" + return self.pos + def signed_lateral_error(self, p) -> float: """Returns the signed lateral distance from the given point to the line formed by the waypoint position and the waypoint heading. @@ -860,6 +874,74 @@ def dist_to(self, p) -> float: return np.linalg.norm(self.pos - p[: len(self.pos)]) +def interpolate_waypoints( + first_waypoint: Waypoint, + second_waypoint: Waypoint, + start_relative_offset: float, + interval: float, +) -> Tuple[List[Waypoint], float]: + """Generate intermediary waypoints between the given waypoints. + + Args: + first_waypoint (Waypoint): The initial start waypoint. + second_waypoint (Waypoint): The initial end waypoint. + start_relative_offset (float): The starting offset to generate the first waypoint. + interval (float): The interval to generate the waypoints at. + + Returns: + Tuple[List[Waypoint], float]: The resulting waypoints and the used offset + """ + assert interval > 0, f"Instead got {interval =}" + assert start_relative_offset >= 0, f"Instead got {start_relative_offset =}" + base_waypoint_offset_delta = abs( + second_waypoint.lane_offset - first_waypoint.lane_offset + ) + accumulated_relative_offset = 0 + out_waypoints: List[Waypoint] = [] + normalize_factor = 1 / base_waypoint_offset_delta + while ( + accumulated_relative_offset + start_relative_offset + <= base_waypoint_offset_delta + ): + normalized_relative_offset = ( + accumulated_relative_offset + start_relative_offset + ) * normalize_factor + out_waypoints.append( + Waypoint( + pos=lerp( + first_waypoint.pos, second_waypoint.pos, normalized_relative_offset + ), + heading=Heading( + lerp( + first_waypoint.heading, + second_waypoint.heading, + normalized_relative_offset, + ) + ), + lane_id=first_waypoint.lane_id, + lane_width=lerp( + first_waypoint.lane_width, + second_waypoint.lane_width, + normalized_relative_offset, + ), + speed_limit=lerp( + first_waypoint.lane_width, + second_waypoint.lane_width, + normalized_relative_offset, + ), + lane_index=first_waypoint.lane_index, + lane_offset=lerp( + first_waypoint.lane_offset, + second_waypoint.lane_offset, + normalized_relative_offset, + ), + ) + ) + accumulated_relative_offset += interval + used = base_waypoint_offset_delta + return out_waypoints, used + + class RoadMapWithCaches(RoadMap): """Base class for map implementations that wish to include a built-in SegmentCache and other LRU caches.""" diff --git a/smarts/core/scenario.py b/smarts/core/scenario.py index 54c3000f89..7dcb63e84d 100644 --- a/smarts/core/scenario.py +++ b/smarts/core/scenario.py @@ -17,6 +17,8 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import glob import logging import os @@ -28,6 +30,7 @@ from itertools import cycle, product from pathlib import Path from typing import ( + TYPE_CHECKING, Any, Callable, Dict, @@ -49,7 +52,7 @@ from smarts.core.plan import ( EndlessGoal, LapMission, - Mission, + NavigationMission, PositionalGoal, Start, TraverseGoal, @@ -57,7 +60,6 @@ Via, default_entry_tactic, ) -from smarts.core.road_map import RoadMap from smarts.core.traffic_history import TrafficHistory from smarts.core.utils.core_math import ( combination_pairs_with_unique_indices, @@ -82,6 +84,9 @@ ) import trimesh # only suppress the warnings caused by trimesh +if TYPE_CHECKING: + from smarts.core.road_map import RoadMap + class Scenario: """The purpose of the Scenario is to provide an aggregate of all @@ -105,7 +110,7 @@ def __init__( self, scenario_root: str, traffic_specs: Sequence[str] = [], - missions: Optional[Dict[str, Mission]] = None, + missions: Optional[Dict[str, NavigationMission]] = None, social_agents: Optional[Dict[str, Tuple[Any, SocialAgent]]] = None, log_dir: Optional[str] = None, surface_patches: Optional[Sequence[Dict[str, Any]]] = None, @@ -361,8 +366,8 @@ def discover_friction_map(scenario_root) -> List[Dict[str, Any]]: @staticmethod @lru_cache(maxsize=16) def _discover_social_agents_info( - scenario, - ) -> Sequence[Dict[str, Tuple[SocialAgent, Mission]]]: + scenario: Union[str, Scenario], + ) -> Sequence[Dict[str, Tuple[SocialAgent, NavigationMission]]]: """Loops through the social agent mission pickles, instantiating corresponding implementations for the given types. The output is a list of {agent_id: (mission, locator)}, where each dictionary corresponds to the @@ -559,7 +564,7 @@ def _discover_metadata(scenario_root): metadata = yaml.load(f, yaml.Loader) return metadata - def set_ego_missions(self, ego_missions: Dict[str, Mission]): + def set_ego_missions(self, ego_missions: Dict[str, NavigationMission]): """Replaces the ego missions within the scenario. Args: ego_missions: Ego agent ids mapped to missions. @@ -597,7 +602,7 @@ def get_vehicle_goal(self, vehicle_id: str) -> Point: final_pos_x, final_pos_y, _, _ = final_pose return Point(final_pos_x, final_pos_y) - def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]: + def discover_missions_of_traffic_histories(self) -> Dict[str, NavigationMission]: """Retrieves the missions of traffic history vehicles.""" vehicle_missions = {} for row in self._traffic_history.first_seen_times(): @@ -607,7 +612,7 @@ def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]: entry_tactic = default_entry_tactic(speed) veh_config_type = self._traffic_history.vehicle_config_type(v_id) veh_dims = self._traffic_history.vehicle_dims(v_id) - vehicle_missions[v_id] = Mission( + vehicle_missions[v_id] = NavigationMission( start=start, entry_tactic=entry_tactic, goal=TraverseGoal(self.road_map), @@ -622,7 +627,7 @@ def discover_missions_of_traffic_histories(self) -> Dict[str, Mission]: def create_dynamic_traffic_history_mission( self, vehicle_id: str, trigger_time: float, positional_radius: int - ) -> Tuple[Mission, Mission]: + ) -> Tuple[NavigationMission, NavigationMission]: """Builds missions out of the given vehicle information. Args: vehicle_id: @@ -632,7 +637,7 @@ def create_dynamic_traffic_history_mission( positional_radius: The goal radius for the positional goal. Returns: - (smarts.core.plan.Mission, smarts.core.plan.Mission): A positional mission that follows the initial + (smarts.core.plan.NavigationMission, smarts.core.plan.NavigationMission): A positional mission that follows the initial original vehicle's travel as well as a traverse style mission which is done when the vehicle leaves the map. """ @@ -640,12 +645,12 @@ def create_dynamic_traffic_history_mission( veh_goal = self.get_vehicle_goal(vehicle_id) entry_tactic = default_entry_tactic(speed) # create a positional mission and a traverse mission - positional_mission = Mission( + positional_mission = NavigationMission( start=start, entry_tactic=entry_tactic, goal=PositionalGoal(veh_goal, radius=positional_radius), ) - traverse_mission = Mission( + traverse_mission = NavigationMission( start=start, entry_tactic=entry_tactic, goal=TraverseGoal(self._road_map), @@ -663,7 +668,7 @@ def history_missions_for_window( Iterable[VehicleWindow], ] ] = None, - ) -> Sequence[Mission]: + ) -> Sequence[NavigationMission]: """Discovers vehicle missions for the given window of time. :param exists_at_or_after: The starting time of any vehicles to query for. @@ -677,7 +682,7 @@ def history_missions_for_window( :param filter: A filter in the form of ``(func(Sequence[TrafficHistoryVehicleWindow]) -> Sequence[TrafficHistoryVehicleWindow])``, which passes in traffic vehicle information and then should be used purely to filter the sequence down. :return: A set of missions derived from the traffic history. - :rtype: List[smarts.core.plan.Mission] + :rtype: List[smarts.core.plan.NavigationMission] """ vehicle_windows = self._traffic_history.vehicle_windows_in_range( exists_at_or_after, ends_before, minimum_vehicle_window @@ -696,7 +701,7 @@ def _gen_mission(vw: TrafficHistory.TrafficHistoryVehicleWindow): entry_tactic = default_entry_tactic(vw.start_speed) veh_config_type = vw.vehicle_type veh_dims = vw.dimensions - vehicle_mission = Mission( + vehicle_mission = NavigationMission( start=start, entry_tactic=entry_tactic, goal=TraverseGoal(self.road_map), @@ -796,7 +801,7 @@ def to_scenario_via( entry_tactic = mission.entry_tactic start_time = Scenario._extract_mission_start_time(mission, entry_tactic) - return Mission( + return NavigationMission( start=start, route_vias=mission.route.via, goal=goal, @@ -813,7 +818,7 @@ def to_scenario_via( entry_tactic = mission.entry_tactic start_time = Scenario._extract_mission_start_time(mission, entry_tactic) - return Mission( + return NavigationMission( start=start, goal=EndlessGoal(), start_time=start_time, @@ -1063,7 +1068,7 @@ def all_support_sumo_traffic(scenarios: Sequence[str]) -> bool: return True @property - def missions(self) -> Dict[str, Mission]: + def missions(self) -> Dict[str, NavigationMission]: """Agent missions contained within this scenario.""" return self._missions @@ -1077,7 +1082,7 @@ def bubbles(self): """Bubbles within this scenario.""" return self._bubbles - def mission(self, agent_id) -> Optional[Mission]: + def mission(self, agent_id) -> Optional[NavigationMission]: """Get the mission assigned to the given agent.""" return self._missions.get(agent_id, None) diff --git a/smarts/core/sensor.py b/smarts/core/sensor.py index d4d72fa283..96681049ab 100644 --- a/smarts/core/sensor.py +++ b/smarts/core/sensor.py @@ -19,47 +19,88 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + +import abc +import importlib.resources as pkg_resources import logging +import math import sys from collections import deque from dataclasses import dataclass from functools import lru_cache -from typing import List, Optional, Tuple +from typing import TYPE_CHECKING, Any, Collection, Dict, List, Optional, Tuple, Union import numpy as np -from smarts.core.coordinates import Pose, RefLinePoint +from smarts.core import glsl +from smarts.core.actor import ActorState +from smarts.core.agent_interface import ( + CustomRenderBufferDependency, + CustomRenderCameraDependency, + CustomRenderVariableDependency, + RenderDependencyBase, +) +from smarts.core.coordinates import Dimensions, Pose, RefLinePoint from smarts.core.lidar import Lidar -from smarts.core.lidar_sensor_params import SensorParams from smarts.core.masks import RenderMasks from smarts.core.observations import ( + CustomRenderData, DrivableAreaGridMap, GridMapMetadata, + Observation, + OcclusionRender, OccupancyGridMap, RoadWaypoints, SignalObservation, TopDownRGB, ViaPoint, ) -from smarts.core.plan import Plan -from smarts.core.renderer_base import RendererBase +from smarts.core.renderer_base import ( + RendererBase, + ShaderStepBufferDependency, + ShaderStepCameraDependency, + ShaderStepVariableDependency, +) from smarts.core.road_map import RoadMap, Waypoint +from smarts.core.shader_buffer import BufferID, CameraSensorID from smarts.core.signals import SignalState +from smarts.core.simulation_frame import SimulationFrame from smarts.core.utils.core_math import squared_dist -from smarts.core.vehicle_state import VehicleState, neighborhood_vehicles_around_vehicle +from smarts.core.vehicle_state import neighborhood_vehicles_around_vehicle + +if TYPE_CHECKING: + from smarts.core.actor import ActorState + from smarts.core.lidar_sensor_params import SensorParams + from smarts.core.plan import Plan + from smarts.core.provider import ProviderState + from smarts.core.simulation_frame import SimulationFrame + from smarts.core.vehicle_state import VehicleState + +def _gen_sensor_name(base_name: str, vehicle_state: VehicleState): + return _gen_base_sensor_name(base_name, vehicle_state.actor_id) -class Sensor: + +def _gen_base_sensor_name(base_name: str, actor_id: str): + return f"{base_name}_{actor_id}" + + +class Sensor(metaclass=abc.ABCMeta): """The sensor base class.""" - def step(self, sim_frame, **kwargs): + def step(self, sim_frame: SimulationFrame, **kwargs): """Update sensor state.""" - pass + @abc.abstractmethod def teardown(self, **kwargs): """Clean up internal resources""" raise NotImplementedError + @abc.abstractmethod + def __call__(self, *args: Any, **kwds: Any) -> Any: + raise NotImplementedError + @property def mutable(self) -> bool: """If this sensor mutates on call.""" @@ -83,20 +124,24 @@ def __init__( width: int, height: int, resolution: float, + build_camera: bool = True, ): assert renderer self._log = logging.getLogger(self.__class__.__name__) - self._camera_name = renderer.build_offscreen_camera( - f"{name}_{vehicle_state.actor_id}", - mask, - width, - height, - resolution, - ) + self._name = name + self._camera_name = _gen_sensor_name(name, vehicle_state) + if build_camera: + renderer.build_offscreen_camera( + self._camera_name, + mask, + width, + height, + resolution, + ) + self._follow_actor( + vehicle_state, renderer + ) # ensure we have a correct initial camera position self._target_actor = vehicle_state.actor_id - self._follow_actor( - vehicle_state, renderer - ) # ensure we have a correct initial camera position self._mask = mask self._width = width self._height = height @@ -120,18 +165,32 @@ def teardown(self, **kwargs): camera = renderer.camera_for_id(self._camera_name) camera.teardown() - def step(self, sim_frame, **kwargs): + def step(self, sim_frame: SimulationFrame, **kwargs): if not self._target_actor in sim_frame.actor_states_by_id: return self._follow_actor( sim_frame.actor_states_by_id[self._target_actor], kwargs.get("renderer") ) - def _follow_actor(self, vehicle_state: VehicleState, renderer: RendererBase): + def _follow_actor(self, actor_state: ActorState, renderer: RendererBase): if not renderer: return camera = renderer.camera_for_id(self._camera_name) - camera.update(vehicle_state.pose, vehicle_state.dimensions.height + 10) + pose = actor_state.get_pose() + dimensions = actor_state.get_dimensions() + if pose == None or dimensions == None: + return + camera.update(pose, dimensions.height + 10) + + @property + def camera_name(self) -> str: + """The name of the camera this sensor is using.""" + return self._camera_name + + @property + def name(self) -> str: + """The name of this sensor.""" + return self._name @property def serializable(self) -> bool: @@ -152,7 +211,7 @@ def __init__( super().__init__( vehicle_state, renderer, - "drivable_area_grid_map", + CameraSensorID.DRIVABLE_AREA_GRID_MAP.value, RenderMasks.DRIVABLE_AREA_HIDE, width, height, @@ -195,22 +254,21 @@ def __init__( super().__init__( vehicle_state, renderer, - "ogm", + CameraSensorID.OCCUPANCY_GRID_MAP.value, RenderMasks.OCCUPANCY_HIDE, width, height, resolution, ) - self._resolution = resolution def __call__(self, renderer: RendererBase) -> OccupancyGridMap: - camera = renderer.camera_for_id(self._camera_name) - assert camera is not None, "OGM has not been initialized" + base_camera = renderer.camera_for_id(self._camera_name) + assert base_camera is not None, "OGM has not been initialized" - ram_image = camera.wait_for_ram_image(img_format="A") + ram_image = base_camera.wait_for_ram_image(img_format="A") mem_view = memoryview(ram_image) grid: np.ndarray = np.frombuffer(mem_view, np.uint8) - width, height = camera.image_dimensions + width, height = base_camera.image_dimensions grid.shape = (height, width, 1) grid = np.flipud(grid) @@ -218,8 +276,8 @@ def __call__(self, renderer: RendererBase) -> OccupancyGridMap: resolution=self._resolution, height=grid.shape[0], width=grid.shape[1], - camera_position=camera.position, - camera_heading=camera.heading, + camera_position=base_camera.position, + camera_heading=base_camera.heading, ) return OccupancyGridMap(data=grid, metadata=metadata) @@ -238,7 +296,7 @@ def __init__( super().__init__( vehicle_state, renderer, - "top_down_rgb", + CameraSensorID.TOP_DOWN_RGB.value, RenderMasks.RGB_HIDE, width, height, @@ -267,6 +325,263 @@ def __call__(self, renderer: RendererBase) -> TopDownRGB: return TopDownRGB(data=image, metadata=metadata) +class OcclusionMapSensor(CameraSensor): + """A sensor that demonstrates only the areas that can be seen by the vehicle.""" + + def __init__( + self, + vehicle_state: VehicleState, + width: int, + height: int, + resolution: float, + renderer: RendererBase, + ogm_sensor: OGMSensor, + add_surface_noise: bool, + ): + self._effect_cameras = [] + super().__init__( + vehicle_state, + renderer, + CameraSensorID.OCCLUSION.value, + RenderMasks.NONE, + width, + height, + resolution, + build_camera=False, + ) + + occlusion_camera0 = ogm_sensor.camera_name + occlusion_camera1 = occlusion_camera0 + + if add_surface_noise: + # generate simplex camera + with pkg_resources.path(glsl, "simplex_shader.frag") as simplex_shader_path: + simplex_camera_name = _gen_sensor_name("simplex", vehicle_state) + renderer.build_shader_step( + name=simplex_camera_name, + fshader_path=simplex_shader_path, + dependencies=( + ShaderStepVariableDependency( + value=1.0 / resolution, + script_variable_name="scale", + ), + ShaderStepCameraDependency( + _gen_sensor_name( + CameraSensorID.DRIVABLE_AREA_GRID_MAP.value, + vehicle_state, + ), + "iChannel0", + ), + ), + priority=10, + width=width, + height=height, + ) + self._effect_cameras.append(simplex_camera_name) + occlusion_camera1 = simplex_camera_name + + # feed simplex and ogm to composite + with pkg_resources.path(glsl, "occlusion_shader.frag") as composite_shader_path: + composite_camera_name = _gen_sensor_name( + CameraSensorID.OCCLUSION.value, vehicle_state + ) + renderer.build_shader_step( + name=composite_camera_name, + fshader_path=composite_shader_path, + dependencies=( + ShaderStepCameraDependency(occlusion_camera0, "iChannel0"), + ShaderStepCameraDependency(occlusion_camera1, "iChannel1"), + ), + priority=30, + width=width, + height=height, + ) + self._effect_cameras.append(composite_camera_name) + + def _follow_actor(self, actor_state: ActorState, renderer: RendererBase): + if not renderer: + return + for effect_name in self._effect_cameras: + pose = actor_state.get_pose() + dimensions = actor_state.get_dimensions() + if pose == None or dimensions == None: + continue + camera = renderer.camera_for_id(effect_name) + camera.update(pose, dimensions.height) + + def teardown(self, **kwargs): + renderer: Optional[RendererBase] = kwargs.get("renderer") + if not renderer: + return + for effect_name in self._effect_cameras: + camera = renderer.camera_for_id(effect_name) + camera.teardown() + + def __call__(self, renderer: RendererBase) -> OcclusionRender: + effect_camera = renderer.camera_for_id(self._effect_cameras[-1]) + + ram_image = effect_camera.wait_for_ram_image("RGB") + mem_view = memoryview(ram_image) + grid: np.ndarray = np.frombuffer(mem_view, np.uint8)[::3] + grid.shape = effect_camera.image_dimensions + grid = np.flipud(grid) + + metadata = GridMapMetadata( + resolution=self._resolution, + height=grid.shape[0], + width=grid.shape[1], + camera_position=(math.inf, math.inf, math.inf), + camera_heading=math.inf, + ) + return OcclusionRender(data=grid, metadata=metadata) + + +class CustomRenderSensor(CameraSensor): + """Defines a configurable image sensor.""" + + def __init__( + self, + vehicle_state: VehicleState, + width: int, + height: int, + resolution: float, + renderer: RendererBase, + fragment_shader_path: str, + render_dependencies: Tuple[RenderDependencyBase, ...], + ogm_sensor: Optional[OGMSensor], + top_down_rgb_sensor: Optional[RGBSensor], + drivable_area_grid_map_sensor: Optional[DrivableAreaGridMapSensor], + occlusion_map_sensor: Optional[OcclusionMapSensor], + name: str, + ): + super().__init__( + vehicle_state, + renderer, + name, + RenderMasks.NONE, + width, + height, + resolution, + build_camera=False, + ) + + dependencies = [] + named_camera_sensors = ( + (CameraSensorID.OCCUPANCY_GRID_MAP, ogm_sensor), + (CameraSensorID.TOP_DOWN_RGB, top_down_rgb_sensor), + (CameraSensorID.DRIVABLE_AREA_GRID_MAP, drivable_area_grid_map_sensor), + (CameraSensorID.OCCLUSION, occlusion_map_sensor), + ) + + def has_required(dependency_name, required_name, sensor) -> bool: + if dependency_name == required_name: + if sensor: + return True + raise UserWarning( + f"Custom render depency requires `{d.name}` but the sensor is not attached in the interface." + ) + return False + + for d in render_dependencies: + if isinstance(d, CustomRenderCameraDependency): + for csn, sensor in named_camera_sensors: + if has_required(d.camera_dependency_name, csn.value, sensor): + break + + camera_id = ( + _gen_sensor_name(d.camera_dependency_name, vehicle_state) + if d.is_self_targetted() + else _gen_base_sensor_name(d.camera_dependency_name, d.target_actor) + ) + dependency = ShaderStepCameraDependency( + camera_id, + d.variable_name, + ) + elif isinstance(d, CustomRenderVariableDependency): + dependency = ShaderStepVariableDependency( + value=d.value, + script_variable_name=d.variable_name, + ) + elif isinstance(d, CustomRenderBufferDependency): + if isinstance(d.buffer_dependency_name, str): + buffer_name = BufferID(d.buffer_dependency_name) + else: + buffer_name = d.buffer_dependency_name + + dependency = ShaderStepBufferDependency( + buffer_id=buffer_name, + script_uniform_name=d.variable_name, + ) + else: + raise TypeError( + f"Dependency must be a subtype of `{RenderDependencyBase}`" + ) + dependencies.append(dependency) + + renderer.build_shader_step( + name=self._camera_name, + fshader_path=fragment_shader_path, + dependencies=dependencies, + priority=40, + width=width, + height=height, + ) + + def step(self, sim_frame: SimulationFrame, **kwargs): + if not self._target_actor in sim_frame.actor_states_by_id: + return + actor_state = sim_frame.actor_states_by_id[self._target_actor] + renderer = kwargs.get("renderer") + observations: Optional[Dict[str, Observation]] = kwargs.get("observations") + + target = None + if isinstance(observations, dict): + for k, o in observations.items(): + o: Observation + if o.ego_vehicle_state.id == self._target_actor: + target = o + assert isinstance(target, Observation) + + if not renderer: + return + renderer: RendererBase + camera = renderer.camera_for_id(self._camera_name) + pose = actor_state.get_pose() + dimensions = actor_state.get_dimensions() + if not target: + camera.update( + pose=pose, height=(dimensions.height + 10) if dimensions else None + ) + else: + camera.update(observation=target) + + def teardown(self, **kwargs): + renderer = kwargs.get("renderer") + if not renderer: + return + renderer: RendererBase + camera = renderer.camera_for_id(self._camera_name) + camera.teardown() + + def __call__(self, renderer: RendererBase) -> CustomRenderData: + effect_camera = renderer.camera_for_id(self._camera_name) + + ram_image = effect_camera.wait_for_ram_image("RGB") + mem_view = memoryview(ram_image) + grid: np.ndarray = np.frombuffer(mem_view, np.uint8) + grid.shape = effect_camera.image_dimensions + (3,) + grid = np.flipud(grid) + + metadata = GridMapMetadata( + resolution=self._resolution, + height=grid.shape[0], + width=grid.shape[1], + camera_position=(math.inf, math.inf, math.inf), # has no position + camera_heading=math.inf, # has no heading + ) + return CustomRenderData(data=grid, metadata=metadata) + + class LidarSensor(Sensor): """A lidar sensor.""" @@ -382,7 +697,11 @@ def __eq__(self, __value: object) -> bool: ) def update_distance_wps_record( - self, waypoint_paths, vehicle, plan: Plan, road_map: RoadMap + self, + waypoint_paths: List[List[Waypoint]], + vehicle_state: VehicleState, + plan: Plan, + road_map: RoadMap, ): """Append a waypoint to the history if it is not already counted.""" # Distance calculation. Intention is the shortest trip travelled at the lane @@ -402,7 +721,7 @@ def update_distance_wps_record( ) if not self._wps_for_distance: - self._last_actor_position = vehicle.pose.position + self._last_actor_position = vehicle_state.pose.position if should_count_wp: self._wps_for_distance.append(new_wp) return # sensor does not have enough history @@ -418,11 +737,11 @@ def update_distance_wps_record( additional_distance = TripMeterSensor._compute_additional_dist_travelled( most_recent_wp, new_wp, - vehicle.pose.position, + vehicle_state.pose.position, self._last_actor_position, ) self._dist_travelled += additional_distance - self._last_actor_position = vehicle.pose.position + self._last_actor_position = vehicle_state.pose.position @staticmethod def _compute_additional_dist_travelled( @@ -441,7 +760,7 @@ def _compute_additional_dist_travelled( distance = np.dot(position_disp_vec, wp_unit_vec) return distance - def __call__(self, increment=False): + def __call__(self, increment: bool = False): if increment: return self._dist_travelled - self._last_dist_travelled @@ -454,15 +773,17 @@ def teardown(self, **kwargs): class NeighborhoodVehiclesSensor(Sensor): """Detects other vehicles around the sensor equipped vehicle.""" - def __init__(self, radius=None): + def __init__(self, radius: Optional[float] = None): self._radius = radius @property - def radius(self): + def radius(self) -> float: """Radius to check for nearby vehicles.""" return self._radius - def __call__(self, vehicle_state: VehicleState, vehicle_states): + def __call__( + self, vehicle_state: VehicleState, vehicle_states: Collection[VehicleState] + ) -> List[VehicleState]: return neighborhood_vehicles_around_vehicle( vehicle_state, vehicle_states, radius=self._radius ) @@ -484,10 +805,10 @@ def mutable(self) -> bool: class WaypointsSensor(Sensor): """Detects waypoints leading forward along the vehicle plan.""" - def __init__(self, lookahead=32): + def __init__(self, lookahead: int = 32): self._lookahead = lookahead - def __call__(self, vehicle_state: VehicleState, plan: Plan, road_map): + def __call__(self, vehicle_state: VehicleState, plan: Plan, road_map: RoadMap): return road_map.waypoint_paths( pose=vehicle_state.pose, lookahead=self._lookahead, @@ -511,10 +832,12 @@ def mutable(self) -> bool: class RoadWaypointsSensor(Sensor): """Detects waypoints from all paths nearby the vehicle.""" - def __init__(self, horizon=32): + def __init__(self, horizon: int = 32): self._horizon = horizon - def __call__(self, vehicle_state: VehicleState, plan, road_map) -> RoadWaypoints: + def __call__( + self, vehicle_state: VehicleState, plan: Plan, road_map: RoadMap + ) -> RoadWaypoints: veh_pt = vehicle_state.pose.point lane = road_map.nearest_lane(veh_pt) if not lane: @@ -532,7 +855,11 @@ def __call__(self, vehicle_state: VehicleState, plan, road_map) -> RoadWaypoints return RoadWaypoints(lanes=lane_paths) def _paths_for_lane( - self, lane, vehicle_state: VehicleState, plan, overflow_offset=None + self, + lane: RoadMap.Lane, + vehicle_state: VehicleState, + plan: Plan, + overflow_offset: Optional[float] = None, ): """Gets waypoint paths along the given lane.""" # XXX: the following assumes waypoint spacing is 1m @@ -543,22 +870,21 @@ def _paths_for_lane( start_offset = lane.length + overflow_offset incoming_lanes = lane.incoming_lanes + paths = [] if start_offset < 0 and len(incoming_lanes) > 0: - paths = [] for lane in incoming_lanes: paths += self._paths_for_lane(lane, vehicle_state, plan, start_offset) - return paths - else: - start_offset = max(0, start_offset) - wp_start = lane.from_lane_coord(RefLinePoint(start_offset)) - adj_pose = Pose.from_center(wp_start, vehicle_state.pose.heading) - wps_to_lookahead = self._horizon * 2 - paths = lane.waypoint_paths_for_pose( - pose=adj_pose, - lookahead=wps_to_lookahead, - route=plan.route, - ) - return paths + + start_offset = max(0, start_offset) + wp_start = lane.from_lane_coord(RefLinePoint(start_offset)) + adj_pose = Pose.from_center(wp_start, vehicle_state.pose.heading) + wps_to_lookahead = self._horizon * 2 + paths += lane.waypoint_paths_for_pose( + pose=adj_pose, + lookahead=wps_to_lookahead, + route=plan.route, + ) + return paths def __eq__(self, __value: object) -> bool: return isinstance(__value, RoadWaypoints) and self._horizon == __value._horizon @@ -633,7 +959,7 @@ class LanePositionSensor(Sensor): def __init__(self): pass - def __call__(self, lane: RoadMap.Lane, vehicle_state): + def __call__(self, lane: RoadMap.Lane, vehicle_state: VehicleState): return lane.to_lane_coord(vehicle_state.pose.point) def __eq__(self, __value: object) -> bool: @@ -662,12 +988,11 @@ def __eq__(self, __value: object) -> bool: and self._speed_accuracy == __value._speed_accuracy ) - def __call__(self, vehicle_state: VehicleState, plan, road_map): - near_points: List[ViaPoint] = list() - hit_points: List[ViaPoint] = list() + def __call__(self, vehicle_state: VehicleState, plan: Plan, road_map: RoadMap): if plan.mission is None: - return (near_points, hit_points) + return () + near_points: List[Tuple[float, ViaPoint]] = [] vehicle_position = vehicle_state.pose.position[:2] @lru_cache() @@ -685,32 +1010,29 @@ def closest_point_on_lane(position, lane_id, road_map): if dist_from_lane_sq > self._acquisition_range**2: continue + dist_from_point_sq = squared_dist(vehicle_position, via.position) + hit = ( + dist_from_point_sq <= via.hit_distance**2 + and via not in self._consumed_via_points + and np.isclose( + vehicle_state.speed, via.required_speed, atol=self._speed_accuracy + ) + ) + point = ViaPoint( tuple(via.position), lane_index=via.lane_index, road_id=via.road_id, required_speed=via.required_speed, + hit=hit, ) - near_points.append(point) - dist_from_point_sq = squared_dist(vehicle_position, via.position) - if ( - dist_from_point_sq <= via.hit_distance**2 - and via not in self._consumed_via_points - and np.isclose( - vehicle_state.speed, via.required_speed, atol=self._speed_accuracy - ) - ): + near_points.append((dist_from_point_sq, point)) + if hit: self._consumed_via_points.add(via) - hit_points.append(point) - return ( - sorted( - near_points, - key=lambda point: squared_dist(point.position, vehicle_position), - ), - hit_points, - ) + near_points.sort(key=lambda dist, _: dist) + return tuple(p for _, p in near_points) def teardown(self, **kwargs): pass @@ -740,11 +1062,11 @@ def __call__( lane_pos: RefLinePoint, state: VehicleState, plan: Plan, - provider_state, # ProviderState - ) -> List[SignalObservation]: - result = [] + provider_state: ProviderState, + ) -> Tuple[SignalObservation, ...]: if not lane: - return result + return () + result = [] upcoming_signals = [] for feat in lane.features: if not self._is_signal_type(feat): @@ -779,12 +1101,12 @@ def __call__( SignalObservation( state=signal_state.state, stop_point=signal_state.stopping_pos, - controlled_lanes=controlled_lanes, + controlled_lanes=tuple(controlled_lanes), last_changed=signal_state.last_changed, ) ) - return result + return tuple(result) def _find_signals_ahead( self, diff --git a/smarts/core/sensor_manager.py b/smarts/core/sensor_manager.py index c00c323245..2f89b21a4b 100644 --- a/smarts/core/sensor_manager.py +++ b/smarts/core/sensor_manager.py @@ -19,25 +19,37 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging from collections import Counter -from typing import Dict, Iterable, List, Optional, Set, Tuple +from typing import ( + TYPE_CHECKING, + Collection, + Dict, + Iterable, + Iterator, + List, + Optional, + Set, + Tuple, +) from smarts.core import config -from smarts.core.agent_interface import AgentInterface -from smarts.core.renderer_base import RendererBase -from smarts.core.sensors import ( - Observation, - Sensor, - SensorResolver, - Sensors, - SensorState, -) +from smarts.core.observations import Observation +from smarts.core.sensors import SensorResolver, Sensors, SensorState from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver from smarts.core.sensors.parallel_sensor_resolver import ParallelSensorResolver from smarts.core.simulation_frame import SimulationFrame from smarts.core.simulation_local_constants import SimulationLocalConstants +if TYPE_CHECKING: + from smarts.core.agent_interface import AgentInterface + from smarts.core.renderer_base import RendererBase + from smarts.core.sensor import Sensor + from smarts.core.utils.pybullet import bullet_client as bc + from smarts.core.vehicle import Vehicle + class SensorManager: """A sensor management system that associates actors with sensors.""" @@ -59,9 +71,7 @@ def __init__(self): "core", "observation_workers", default=0, cast=int ) parallel_resolver = ParallelSensorResolver - if ( - backing := config()("core", "sensor_parallelization", default="mp") - ) == "ray": + if (backing := config()("core", "sensor_parallelization")) == "ray": try: import ray @@ -80,7 +90,7 @@ def __init__(self): parallel_resolver() if observation_workers > 0 else LocalSensorResolver() ) - def step(self, sim_frame: SimulationFrame, renderer): + def step(self, sim_frame: SimulationFrame, renderer: RendererBase): """Update sensor values based on the new simulation state.""" self._sensor_resolver.step(sim_frame, self._sensor_states.values()) @@ -89,12 +99,12 @@ def step(self, sim_frame: SimulationFrame, renderer): def observe( self, - sim_frame, - sim_local_constants, - agent_ids, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], renderer_ref: RendererBase, - physics_ref, - ): + physics_ref: bc.BulletClient, + ) -> Tuple[Dict[str, Observation], Dict[str, bool]]: """Runs observations and updates the sensor states. Args: sim_frame (SimulationFrame): @@ -130,10 +140,10 @@ def observe_batch( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, interface: AgentInterface, - sensor_states, - vehicles, - renderer, - bullet_client, + sensor_states: Dict[str, SensorState], + vehicles: Dict[str, Vehicle], + renderer: RendererBase, + bullet_client: bc.BulletClient, ) -> Tuple[Dict[str, Observation], Dict[str, bool]]: """Operates all sensors on a batch of vehicles for a single agent.""" # TODO: Replace this with a more efficient implementation that _actually_ @@ -165,7 +175,7 @@ def observe_batch( return observations, dones - def teardown(self, renderer): + def teardown(self, renderer: RendererBase): """Tear down the current sensors and clean up any internal resources.""" self._logger.info("++ Sensors and sensor states reset. ++") for sensor in self._sensors.values(): @@ -225,7 +235,7 @@ def remove_sensor( self._disassociate_sensor(sensor_id, schedule_teardown) return sensor - def _disassociate_sensor(self, sensor_id, schedule_teardown): + def _disassociate_sensor(self, sensor_id: str, schedule_teardown: bool): if schedule_teardown: self._scheduled_sensors.append((sensor_id, self._sensors[sensor_id])) @@ -244,9 +254,9 @@ def sensor_state_exists(self, actor_id: str) -> bool: """Determines if a actor has a sensor state associated with it.""" return actor_id in self._sensor_states - def sensor_states_items(self): + def sensor_states_items(self) -> Iterator[Tuple[str, SensorState]]: """Gets all actor to sensor state associations.""" - return self._sensor_states.items() + return map(lambda x: x, self._sensor_states.items()) def sensors_for_actor_id(self, actor_id: str) -> List[Sensor]: """Gets all sensors associated with the given actor.""" @@ -267,16 +277,16 @@ def sensors_for_actor_ids( for actor_id in actor_ids } - def sensor_state_for_actor_id(self, actor_id: str): + def sensor_state_for_actor_id(self, actor_id: str) -> Optional[SensorState]: """Gets the sensor state for the given actor.""" return self._sensor_states.get(actor_id) @staticmethod - def _actor_sid_to_sname(sensor_id: str): + def _actor_sid_to_sname(sensor_id: str) -> str: return sensor_id.partition("-")[0] @staticmethod - def _actor_and_sensor_name_to_sensor_id(sensor_name, actor_id): + def _actor_and_sensor_name_to_sensor_id(sensor_name: str, actor_id: str) -> str: return f"{sensor_name}-{actor_id}" def add_sensor_for_actor(self, actor_id: str, name: str, sensor: Sensor) -> str: @@ -302,7 +312,9 @@ def add_sensor(self, sensor_id, sensor: Sensor) -> str: self._sensor_references.update([sensor_id]) return sensor_id - def clean_up_sensors_for_actors(self, current_actor_ids: Set[str], renderer): + def clean_up_sensors_for_actors( + self, current_actor_ids: Set[str], renderer: RendererBase + ): """Cleans up sensors that are attached to non-existing actors.""" # This is not good enough by itself since actors can keep alive sensors that are not in use by an agent old_actor_ids = set(self._sensor_states) diff --git a/smarts/core/sensors/__init__.py b/smarts/core/sensors/__init__.py index 2509425a7b..d0320fdd62 100644 --- a/smarts/core/sensors/__init__.py +++ b/smarts/core/sensors/__init__.py @@ -23,15 +23,22 @@ import math import re import sys -from typing import Any, Dict, List, Optional, Sequence, Set, Tuple, Type +from typing import ( + TYPE_CHECKING, + Any, + Collection, + Dict, + Iterable, + List, + Optional, + Sequence, + Set, + Tuple, + Type, +) import numpy as np -from smarts.core.agent_interface import ( - AgentInterface, - AgentsAliveDoneCriteria, - InterestDoneCriteria, -) from smarts.core.coordinates import Heading, Point from smarts.core.events import Events from smarts.core.observations import ( @@ -49,10 +56,10 @@ ) from smarts.core.plan import Plan, PlanFrame from smarts.core.renderer_base import RendererBase -from smarts.core.road_map import RoadMap from smarts.core.sensor import ( AccelerometerSensor, CameraSensor, + CustomRenderSensor, DrivableAreaGridMapSensor, DrivenPathSensor, LanePositionSensor, @@ -67,9 +74,20 @@ ViaSensor, WaypointsSensor, ) -from smarts.core.simulation_frame import SimulationFrame -from smarts.core.simulation_local_constants import SimulationLocalConstants -from smarts.core.vehicle_state import VehicleState +from smarts.core.utils.core_logging import timeit + +if TYPE_CHECKING: + from smarts.core.agent_interface import ( + AgentInterface, + AgentsAliveDoneCriteria, + InterestDoneCriteria, + ) + from smarts.core.road_map import RoadMap + from smarts.core.simulation_frame import SimulationFrame + from smarts.core.simulation_local_constants import SimulationLocalConstants + from smarts.core.utils.pybullet import bullet_client as bc + from smarts.core.vehicle import Vehicle + from smarts.core.vehicle_state import VehicleState logger = logging.getLogger(__name__) @@ -79,7 +97,7 @@ def _make_vehicle_observation( - road_map, + road_map: RoadMap, neighborhood_vehicle: VehicleState, sim_frame: SimulationFrame, interest_extension: Optional[re.Pattern], @@ -96,7 +114,7 @@ def _make_vehicle_observation( return VehicleObservation( id=neighborhood_vehicle.actor_id, - position=neighborhood_vehicle.pose.position, + position=neighborhood_vehicle.pose.position_tuple, bounding_box=neighborhood_vehicle.dimensions, heading=neighborhood_vehicle.pose.heading, speed=neighborhood_vehicle.speed, @@ -169,14 +187,15 @@ class SensorResolver: """An interface describing sensor observation and update systems.""" # TODO: Remove renderer and bullet client from the arguments + # TODO: Remove updated sensors from the return. No sensors should be modified in observe!!! def observe( self, sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, agent_ids: Set[str], renderer: RendererBase, - bullet_client, - ): + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Observation], Dict[str, bool], Dict[str, Dict[str, Sensor]]]: """Generate observations Args: @@ -188,10 +207,73 @@ def observe( """ raise NotImplementedError() - def step(self, sim_frame, sensor_states): + def step(self, sim_frame: SimulationFrame, sensor_states: Iterable[SensorState]): """Step the sensor state.""" raise NotImplementedError() + def _gen_phys_observations( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + bullet_client: bc.BulletClient, + updated_sensors: Dict[str, Dict[str, Sensor]], + ) -> Dict[str, Dict[str, Any]]: + with timeit("physics sensors", logger.debug): + phys_observations: Dict[str, Dict[str, Any]] = {} + for agent_id in agent_ids: + for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: + ( + phys_observations[agent_id], + updated_phys_sensors, + ) = Sensors.process_physics_sensors( + sim_frame, + sim_local_constants, + sim_frame.sensor_states[vehicle_id], + vehicle_id, + bullet_client, + ) + updated_sensors[vehicle_id].update(updated_phys_sensors) + + return phys_observations + + def _gen_rendered_observations( + self, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + agent_ids: Set[str], + renderer: RendererBase, + updated_sensors: Dict[str, Dict[str, Sensor]], + ): + with timeit("rendered observations", logger.debug): + rendering_observations = {} + for agent_id in agent_ids: + for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: + ( + rendering_observations[agent_id], + updated_unsafe_sensors, + ) = Sensors.process_rendering_sensors( + sim_frame, + sim_local_constants, + sim_frame.agent_interfaces[agent_id], + sim_frame.sensor_states[vehicle_id], + vehicle_id, + renderer, + ) + updated_sensors[vehicle_id].update(updated_unsafe_sensors) + return rendering_observations + + def _sync_custom_camera_sensors( + self, sim_frame: SimulationFrame, renderer: RendererBase, observations + ): + for v_id, sensors in sim_frame.vehicle_sensors.items(): + for s_id, sensor in sensors.items(): + if sensor.serializable or not isinstance(sensor, CustomRenderSensor): + continue + sensor.step( + sim_frame=sim_frame, renderer=renderer, observations=observations + ) + class Sensors: """Sensor related utilities""" @@ -201,8 +283,8 @@ def observe_serializable_sensor_batch( cls, sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, - agent_ids_for_group, - ): + agent_ids_for_group: Iterable[str], + ) -> Tuple[Dict[str, Observation], Dict[str, bool], Dict[str, Dict[str, Sensor]]]: """Run the serializable sensors in a batch.""" observations, dones, updated_sensors = {}, {}, {} for agent_id in agent_ids_for_group: @@ -226,36 +308,28 @@ def observe_serializable_sensor_batch( return observations, dones, updated_sensors @staticmethod - def process_serialization_unsafe_sensors( + def process_rendering_sensors( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, - interface, - sensor_state, - vehicle_id, - renderer, - bullet_client, - ): + interface: AgentInterface, + sensor_state: SensorState, + vehicle_id: str, + renderer: RendererBase, + ) -> Tuple[Dict[str, Any], Dict[str, Sensor]]: """Run observations that can only be done on the main thread.""" - vehicle_sensors: Dict[str, Any] = sim_frame.vehicle_sensors[vehicle_id] - - vehicle_state = sim_frame.vehicle_states[vehicle_id] - lidar = None - lidar_sensor = vehicle_sensors.get("lidar_sensor") - if lidar_sensor: - lidar_sensor.follow_vehicle(vehicle_state) - lidar = lidar_sensor(bullet_client) + vehicle_sensors = sim_frame.vehicle_sensors[vehicle_id] - def get_camera_sensor_result(sensors, sensor_name, renderer): - return ( - sensors[sensor_name](renderer=renderer) - if renderer and sensors.get(sensor_name) - else None - ) + def get_camera_sensor_result( + sensors: Dict[str, Sensor], sensor_name: str, renderer: RendererBase + ): + if (sensor := sensors.get(sensor_name)) is not None: + return sensor(renderer=renderer) + return None updated_sensors = { sensor_name: sensor for sensor_name, sensor in vehicle_sensors.items() - if sensor.mutable and not sensor.serializable + if isinstance(sensor, CameraSensor) } return ( @@ -269,20 +343,85 @@ def get_camera_sensor_result(sensors, sensor_name, renderer): top_down_rgb=get_camera_sensor_result( vehicle_sensors, "rgb_sensor", renderer ), + occlusion_map=get_camera_sensor_result( + vehicle_sensors, "occlusion_map_sensor", renderer + ), + custom_renders=tuple( + get_camera_sensor_result( + vehicle_sensors, f"custom_render{i}_sensor", renderer + ) + for i, _ in enumerate(interface.custom_renders) + ), + ), + updated_sensors, + ) + + @staticmethod + def process_physics_sensors( + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + sensor_state: SensorState, + vehicle_id: str, + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Any], Dict[str, Sensor]]: + """Run observations that can only be done on the main thread.""" + vehicle_sensors = sim_frame.vehicle_sensors[vehicle_id] + + vehicle_state = sim_frame.vehicle_states[vehicle_id] + lidar = None + updated_sensors = {} + lidar_sensor = vehicle_sensors.get("lidar_sensor") + if lidar_sensor: + lidar_sensor.follow_vehicle(vehicle_state) + lidar = lidar_sensor(bullet_client) + updated_sensors["lidar_sensor"] = lidar_sensor + + return ( + dict( lidar_point_cloud=lidar, ), updated_sensors, ) + @classmethod + def process_serialization_unsafe_sensors( + cls, + sim_frame: SimulationFrame, + sim_local_constants: SimulationLocalConstants, + interface: AgentInterface, + sensor_state: SensorState, + vehicle_id: str, + renderer: RendererBase, + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Any], Dict[str, Sensor]]: + """Run observations that can only be done on the main thread.""" + p_sensors, p_updated_sensors = cls.process_physics_sensors( + sim_frame=sim_frame, + sim_local_constants=sim_local_constants, + sensor_state=sensor_state, + vehicle_id=vehicle_id, + bullet_client=bullet_client, + ) + r_sensors, r_updated_sensors = cls.process_rendering_sensors( + sim_frame=sim_frame, + sim_local_constants=sim_local_constants, + interface=interface, + sensor_state=sensor_state, + vehicle_id=vehicle_id, + renderer=renderer, + ) + + return {**p_sensors, **r_sensors}, {**p_updated_sensors, **r_updated_sensors} + @staticmethod def process_serialization_safe_sensors( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, interface: AgentInterface, sensor_state: SensorState, - vehicle_id, - agent_id=None, - ): + vehicle_id: str, + agent_id: Optional[str] = None, + ) -> Tuple[Observation, bool, Dict[str, Sensor]]: """Observations that can be done on any thread.""" vehicle_sensors = sim_frame.vehicle_sensors[vehicle_id] vehicle_state = sim_frame.vehicle_states[vehicle_id] @@ -366,16 +505,19 @@ def process_serialization_safe_sensors( "linear_jerk", "angular_jerk", ], - acceleration_values, + ( + (None if av is None else tuple(float(f) for f in av)) + for av in acceleration_values + ), ) ) ) ego_vehicle = EgoVehicleObservation( id=vehicle_state.actor_id, - position=vehicle_state.pose.point.as_np_array, + position=vehicle_state.pose.position_tuple, bounding_box=vehicle_state.dimensions, - heading=Heading(vehicle_state.pose.heading), + heading=vehicle_state.pose.heading, speed=vehicle_state.speed, steering=vehicle_state.steering, yaw_rate=vehicle_state.yaw_rate, @@ -383,8 +525,8 @@ def process_serialization_safe_sensors( lane_id=ego_lane_id, lane_index=ego_lane_index, mission=plan.mission, - linear_velocity=vehicle_state.linear_velocity, - angular_velocity=vehicle_state.angular_velocity, + linear_velocity=vehicle_state.linear_velocity_tuple(), + angular_velocity=vehicle_state.angular_velocity_tuple(), lane_position=ego_lane_pos, **acceleration_params, ) @@ -396,18 +538,15 @@ def process_serialization_safe_sensors( else None ) - near_via_points = [] - hit_via_points = [] + near_via_points = () via_sensor = vehicle_sensors.get("via_sensor") if via_sensor: - ( - near_via_points, - hit_via_points, - ) = via_sensor(vehicle_state, plan, sim_local_constants.road_map) + near_via_points = via_sensor( + vehicle_state, plan, sim_local_constants.road_map + ) via_data = Vias( near_via_points=near_via_points, - hit_via_points=hit_via_points, ) distance_travelled = 0 @@ -437,7 +576,6 @@ def process_serialization_safe_sensors( vehicle_state, sensor_state, plan, - vehicle_sensors, ) if done and sensor_state.steps_completed == 1: @@ -474,7 +612,7 @@ def process_serialization_safe_sensors( events=events, ego_vehicle_state=ego_vehicle, under_this_agent_control=agent_controls, - neighborhood_vehicle_states=neighborhood_vehicle_states, + neighborhood_vehicle_states=neighborhood_vehicle_states or (), waypoint_paths=waypoint_paths, distance_travelled=distance_travelled, road_waypoints=road_waypoints, @@ -489,13 +627,13 @@ def process_serialization_safe_sensors( def observe_vehicle( cls, sim_frame: SimulationFrame, - sim_local_constants, - interface, - sensor_state, - vehicle, - renderer, - bullet_client, - ) -> Tuple[Observation, bool, Dict[str, "Sensor"]]: + sim_local_constants: SimulationLocalConstants, + interface: AgentInterface, + sensor_state: SensorState, + vehicle: Vehicle, + renderer: RendererBase, + bullet_client: bc.BulletClient, + ) -> Tuple[Observation, bool, Dict[str, Sensor]]: """Generate observations for the given agent around the given vehicle.""" args = [sim_frame, sim_local_constants, interface, sensor_state, vehicle.id] safe_obs, dones, updated_safe_sensors = cls.process_serialization_safe_sensors( @@ -511,7 +649,10 @@ def observe_vehicle( @classmethod def _agents_alive_done_check( - cls, ego_agent_ids, agent_ids, agents_alive: Optional[AgentsAliveDoneCriteria] + cls, + ego_agent_ids: Collection[str], + agent_ids: Collection[str], + agents_alive: Optional[AgentsAliveDoneCriteria], ): if agents_alive is None: return False @@ -551,7 +692,7 @@ def _agents_alive_done_check( @classmethod def _interest_done_check( cls, - interest_actors, + interest_actors: Collection[str], sensor_state: SensorState, interest_criteria: Optional[InterestDoneCriteria], ): @@ -574,9 +715,8 @@ def _is_done_with_events( sim_local_constants: SimulationLocalConstants, interface: AgentInterface, vehicle_state: VehicleState, - sensor_state, - plan, - vehicle_sensors, + sensor_state: SensorState, + plan: Plan, ): vehicle_sensors = sim_frame.vehicle_sensors[vehicle_state.actor_id] done_criteria = interface.done_criteria @@ -641,7 +781,9 @@ def _is_done_with_events( ) events = Events( - collisions=sim_frame.filtered_vehicle_collisions(vehicle_state.actor_id), + collisions=tuple( + sim_frame.filtered_vehicle_collisions(vehicle_state.actor_id) + ), off_road=is_off_road, reached_goal=reached_goal, reached_max_episode_steps=reached_max_episode_steps, @@ -657,8 +799,12 @@ def _is_done_with_events( @classmethod def _agent_reached_goal( - cls, sensor_state, plan, vehicle_state: VehicleState, trip_meter_sensor - ): + cls, + sensor_state: SensorState, + plan: Plan, + vehicle_state: VehicleState, + trip_meter_sensor: TripMeterSensor, + ) -> bool: if not trip_meter_sensor or plan.mission is None: return False distance_travelled = trip_meter_sensor() @@ -668,10 +814,10 @@ def _agent_reached_goal( @classmethod def _vehicle_is_off_road( cls, - road_map, + road_map: RoadMap, vehicle_state: VehicleState, nearest_lanes: Optional[Sequence["RoadMap.Lane"]] = None, - ): + ) -> bool: return not road_map.road_with_point( vehicle_state.pose.point, lanes_to_search=nearest_lanes ) @@ -679,10 +825,10 @@ def _vehicle_is_off_road( @classmethod def _vehicle_is_on_shoulder( cls, - road_map, + road_map: RoadMap, vehicle_state: VehicleState, nearest_lanes: Optional[Sequence["RoadMap.Lane"]] = None, - ): + ) -> bool: # XXX: this isn't technically right as this would also return True # for vehicles that are completely off road. for corner_coordinate in vehicle_state.bounding_box_points: @@ -695,7 +841,7 @@ def _vehicle_is_on_shoulder( @classmethod def _vehicle_is_not_moving( cls, sim, last_n_seconds_considered, min_distance_moved, driven_path_sensor - ): + ) -> bool: # Flag if the vehicle has been immobile for the past 'last_n_seconds_considered' seconds if sim.elapsed_sim_time < last_n_seconds_considered: return False @@ -716,7 +862,7 @@ def _vehicle_is_off_route_and_wrong_way( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, vehicle_state: VehicleState, - plan, + plan: Plan, nearest_lanes: Optional[Sequence[Tuple[RoadMap.Lane, float]]] = None, ): """Determines if the agent is on route and on the correct side of the road. @@ -787,7 +933,9 @@ def _vehicle_is_off_route_and_wrong_way( return (True, is_wrong_way) @staticmethod - def _vehicle_is_wrong_way(vehicle_state: VehicleState, closest_lane): + def _vehicle_is_wrong_way( + vehicle_state: VehicleState, closest_lane: RoadMap.Lane + ) -> bool: target_pose = closest_lane.center_pose_at_point(vehicle_state.pose.point) # Check if the vehicle heading is oriented away from the lane heading. return ( @@ -796,7 +944,9 @@ def _vehicle_is_wrong_way(vehicle_state: VehicleState, closest_lane): ) @classmethod - def _check_wrong_way_event(cls, lane_to_check, vehicle_state): + def _check_wrong_way_event( + cls, lane_to_check: RoadMap.Lane, vehicle_state: VehicleState + ) -> bool: # When the vehicle is in an intersection, turn off the `wrong way` check to avoid # false positive `wrong way` events. if lane_to_check.in_junction: diff --git a/smarts/core/sensors/local_sensor_resolver.py b/smarts/core/sensors/local_sensor_resolver.py index 7b4e678254..9772b33cfc 100644 --- a/smarts/core/sensors/local_sensor_resolver.py +++ b/smarts/core/sensors/local_sensor_resolver.py @@ -19,15 +19,25 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging -from typing import Set +from typing import TYPE_CHECKING, Dict, Iterable, Optional, Set, Tuple -from smarts.core.sensors import SensorResolver, Sensors -from smarts.core.simulation_frame import SimulationFrame -from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.sensor import CustomRenderSensor +from smarts.core.sensors import SensorResolver, Sensors, SensorState from smarts.core.utils.core_logging import timeit from smarts.core.utils.file import replace +if TYPE_CHECKING: + from smarts.core.observations import Observation + from smarts.core.renderer_base import RendererBase + from smarts.core.sensor import Sensor + from smarts.core.simulation_frame import SimulationFrame + from smarts.core.simulation_local_constants import SimulationLocalConstants + from smarts.core.utils.pybullet import bullet_client as bc + + logger = logging.getLogger(__name__) @@ -39,9 +49,50 @@ def observe( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, agent_ids: Set[str], - renderer, - bullet_client, - ): + renderer: Optional[RendererBase], + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Observation], Dict[str, bool], Dict[str, Dict[str, Sensor]]]: + # If render buffer sensors: + # Call serializable sensors + # Call unserializable non-render results + # Collect unserializable non-render results + # Collect serializable results + # Update renderable sensor buffers + # Render + # Collect renderable sensors + # else: + # Render + # Call serializable sensors + # Collect unserializable non-render sensors + # Collect renderable sensors + # Collect serializable sensors + observations, dones, updated_sensors = self._gen_serialized_obs( + sim_frame, sim_local_constants, agent_ids + ) + phys_observations = self._gen_phys_observations( + sim_frame, sim_local_constants, agent_ids, bullet_client, updated_sensors + ) + + # Merge physics sensor information + for agent_id, p_obs in phys_observations.items(): + observations[agent_id] = replace(observations[agent_id], **p_obs) + + self._sync_custom_camera_sensors(sim_frame, renderer, observations) + + if renderer: + renderer.render() + + rendering_observations = self._gen_rendered_observations( + sim_frame, sim_local_constants, agent_ids, renderer, updated_sensors + ) + + # Merge sensor information + for agent_id, r_obs in rendering_observations.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def _gen_serialized_obs(self, sim_frame, sim_local_constants, agent_ids): with timeit("serial run", logger.debug): ( observations, @@ -53,33 +104,9 @@ def observe( agent_ids, ) - # While observation processes are operating do rendering - with timeit("rendering", logger.debug): - rendering = {} - for agent_id in agent_ids: - for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: - ( - rendering[agent_id], - updated_unsafe_sensors, - ) = Sensors.process_serialization_unsafe_sensors( - sim_frame, - sim_local_constants, - agent_id, - sim_frame.sensor_states[vehicle_id], - vehicle_id, - renderer, - bullet_client, - ) - updated_sensors[vehicle_id].update(updated_unsafe_sensors) - - with timeit(f"merging observations", logger.debug): - # Merge sensor information - for agent_id, r_obs in rendering.items(): - observations[agent_id] = replace(observations[agent_id], **r_obs) - return observations, dones, updated_sensors - def step(self, sim_frame, sensor_states): + def step(self, sim_frame: SimulationFrame, sensor_states: Iterable[SensorState]): """Step the sensor state.""" for sensor_state in sensor_states: sensor_state.step() diff --git a/smarts/core/sensors/parallel_sensor_resolver.py b/smarts/core/sensors/parallel_sensor_resolver.py index 39e51c20f0..648a247201 100644 --- a/smarts/core/sensors/parallel_sensor_resolver.py +++ b/smarts/core/sensors/parallel_sensor_resolver.py @@ -19,23 +19,32 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging import multiprocessing as mp from collections import defaultdict from dataclasses import dataclass from enum import IntEnum -from typing import Any, Dict, List, Optional, Set +from typing import TYPE_CHECKING, Any, Dict, Iterable, List, Optional, Set, Tuple import psutil import smarts.core.serialization.default as serializer from smarts.core import config -from smarts.core.sensors import SensorResolver, Sensors -from smarts.core.simulation_frame import SimulationFrame -from smarts.core.simulation_local_constants import SimulationLocalConstants +from smarts.core.sensors import SensorResolver, Sensors, SensorState from smarts.core.utils.core_logging import timeit from smarts.core.utils.file import replace +if TYPE_CHECKING: + from smarts.core.observations import Observation + from smarts.core.renderer_base import RendererBase + from smarts.core.sensor import Sensor + from smarts.core.simulation_frame import SimulationFrame + from smarts.core.simulation_local_constants import SimulationLocalConstants + from smarts.core.utils.pybullet import bullet_client as bc + + logger = logging.getLogger(__name__) @@ -44,19 +53,19 @@ class ParallelSensorResolver(SensorResolver): def __init__(self, process_count_override: Optional[int] = None) -> None: super().__init__() - self._logger = logging.getLogger("Sensors") + self._logger: logging.Logger = logging.getLogger("Sensors") self._sim_local_constants: SimulationLocalConstants = None self._workers: List[SensorsWorker] = [] - self._process_count_override = process_count_override + self._process_count_override: Optional[int] = process_count_override def observe( self, sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, agent_ids: Set[str], - renderer, - bullet_client, - ): + renderer: RendererBase, + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Observation], Dict[str, bool], Dict[str, Dict[str, Sensor]]]: """Runs observations in parallel where possible. Args: sim_frame (SimulationFrame): @@ -82,12 +91,59 @@ def observe( else max(1, self._process_count_override) ) + used_workers = self._gen_workers_for_serializable_sensors( + sim_frame, sim_local_constants, agent_ids, used_processes + ) + phys_observations = self._gen_phys_observations( + sim_frame, sim_local_constants, agent_ids, bullet_client, updated_sensors + ) + + # Collect futures + with timeit("waiting for observations", logger.debug): + if used_workers: + while agent_ids != set(observations): + assert all( + w.running for w in used_workers + ), "A process worker crashed." + for result in mp.connection.wait( + [worker.connection for worker in used_workers], timeout=5 + ): + # pytype: disable=attribute-error + obs, ds, u_sens = result.recv() + # pytype: enable=attribute-error + observations.update(obs) + dones.update(ds) + for v_id, values in u_sens.items(): + updated_sensors[v_id].update(values) + + # Merge physics sensor information + for agent_id, p_obs in phys_observations.items(): + observations[agent_id] = replace(observations[agent_id], **p_obs) + + self._sync_custom_camera_sensors(sim_frame, renderer, observations) + + if renderer: + renderer.render() + + rendering_observations = self._gen_rendered_observations( + sim_frame, sim_local_constants, agent_ids, renderer, updated_sensors + ) + + # Merge sensor information + for agent_id, r_obs in rendering_observations.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def _gen_workers_for_serializable_sensors( + self, sim_frame, sim_local_constants, agent_ids, used_processes + ): workers: List[SensorsWorker] = self.get_workers( used_processes, sim_local_constants=sim_local_constants ) used_workers: List[SensorsWorker] = [] with timeit( - f"parallizable observations with {len(agent_ids)} and {len(workers)}", + f"setting up parallizable observations with {len(agent_ids)} and {len(workers)}", logger.debug, ): agent_ids_for_grouping = list(agent_ids) @@ -106,50 +162,7 @@ def observe( ) ) used_workers.append(workers[i]) - - # While observation processes are operating do rendering - with timeit("rendering", logger.debug): - rendering = {} - for agent_id in agent_ids: - for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: - ( - rendering[agent_id], - updated_unsafe_sensors, - ) = Sensors.process_serialization_unsafe_sensors( - sim_frame, - sim_local_constants, - agent_id, - sim_frame.sensor_states[vehicle_id], - vehicle_id, - renderer, - bullet_client, - ) - updated_sensors[vehicle_id].update(updated_unsafe_sensors) - - # Collect futures - with timeit("waiting for observations", logger.debug): - if used_workers: - while agent_ids != set(observations): - assert all( - w.running for w in used_workers - ), "A process worker crashed." - for result in mp.connection.wait( - [worker.connection for worker in used_workers], timeout=5 - ): - # pytype: disable=attribute-error - obs, ds, u_sens = result.recv() - # pytype: enable=attribute-error - observations.update(obs) - dones.update(ds) - for v_id, values in u_sens.items(): - updated_sensors[v_id].update(values) - - with timeit(f"merging observations", logger.debug): - # Merge sensor information - for agent_id, r_obs in rendering.items(): - observations[agent_id] = replace(observations[agent_id], **r_obs) - - return observations, dones, updated_sensors + return used_workers def __del__(self): try: @@ -168,7 +181,7 @@ def _validate_configuration(self, local_constants: SimulationLocalConstants): return local_constants == self._sim_local_constants def generate_workers( - self, count, workers_list: List[Any], worker_kwargs: "WorkerKwargs" + self, count: int, workers_list: List[SensorsWorker], worker_kwargs: WorkerKwargs ): """Generate the given number of workers requested.""" while len(workers_list) < count: @@ -182,7 +195,7 @@ def generate_workers( ) def get_workers( - self, count, sim_local_constants: SimulationLocalConstants, **kwargs + self, count: int, sim_local_constants: SimulationLocalConstants, **kwargs ) -> List["SensorsWorker"]: """Get the give number of workers.""" if not self._validate_configuration(sim_local_constants): @@ -195,7 +208,7 @@ def get_workers( self.generate_workers(count, self._workers, worker_kwargs) return self._workers[:count] - def step(self, sim_frame, sensor_states): + def step(self, sim_frame: SimulationFrame, sensor_states: Iterable[SensorState]): """Step the sensor state.""" for sensor_state in sensor_states: sensor_state.step() diff --git a/smarts/core/shader_buffer.py b/smarts/core/shader_buffer.py new file mode 100644 index 0000000000..ad1ed7b99d --- /dev/null +++ b/smarts/core/shader_buffer.py @@ -0,0 +1,120 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from enum import Enum + + +class CameraSensorID(Enum): + """Describes default names for camera configuration.""" + + DRIVABLE_AREA_GRID_MAP = "drivable_area_grid_map" + TOP_DOWN_RGB = "top_down_rgb" + OCCUPANCY_GRID_MAP = "ogm" + OCCLUSION = "occlusion_map" + + +class BufferID(Enum): + """The names of the different buffers available for camera rendering.""" + + DELTA_TIME = "dt" + STEP_COUNT = "step_count" + STEPS_COMPLETED = "steps_completed" + ELAPSED_SIM_TIME = "elapsed_sim_time" + + EVENTS_COLLISIONS = "events_collisions" + EVENTS_OFF_ROAD = "events_off_road" + EVENTS_OFF_ROUTE = "events_off_route" + EVENTS_ON_SHOULDER = "events_on_shoulder" + EVENTS_WRONG_WAY = "events_wrong_way" + EVENTS_NOT_MOVING = "events_not_moving" + EVENTS_REACHED_GOAL = "events_reached_goal" + EVENTS_REACHED_MAX_EPISODE_STEPS = "events_reached_max_episode_steps" + EVENTS_AGENTS_ALIVE_DONE = "events_agents_done_alive" + EVENTS_INTEREST_DONE = "events_interest_done" + + EGO_VEHICLE_STATE_POSITION = "ego_vehicle_state_position" + EGO_VEHICLE_STATE_BOUNDING_BOX = "ego_vehicle_state_bounding_box" + EGO_VEHICLE_STATE_HEADING = "ego_vehicle_state_heading" + EGO_VEHICLE_STATE_SPEED = "ego_vehicle_state_speed" + EGO_VEHICLE_STATE_STEERING = "ego_vehicle_state_steering" + EGO_VEHICLE_STATE_YAW_RATE = "ego_vehicle_state_yaw_rate" + EGO_VEHICLE_STATE_ROAD_ID = "ego_vehicle_state_road_id" + EGO_VEHICLE_STATE_LANE_ID = "ego_vehicle_state_lane_id" + EGO_VEHICLE_STATE_LANE_INDEX = "ego_vehicle_state_lane_index" + EGO_VEHICLE_STATE_LINEAR_VELOCITY = "ego_vehicle_state_linear_velocity" + EGO_VEHICLE_STATE_ANGULAR_VELOCITY = "ego_vehicle_state_angular_velocity" + EGO_VEHICLE_STATE_LINEAR_ACCELERATION = "ego_vehicle_state_linear_acceleration" + EGO_VEHICLE_STATE_ANGULAR_ACCELERATION = "ego_vehicle_state_angular_acceleration" + EGO_VEHICLE_STATE_LINEAR_JERK = "ego_vehicle_state_linear_jerk" + EGO_VEHICLE_STATE_ANGULAR_JERK = "ego_vehicle_state_angular_jerk" + EGO_VEHICLE_STATE_LANE_POSITION = "ego_vehicle_state_lane_position" + + UNDER_THIS_VEHICLE_CONTROL = "under_this_vehicle_control" + + NEIGHBORHOOD_VEHICLE_STATES_POSITION = "neighborhood_vehicle_states_position" + NEIGHBORHOOD_VEHICLE_STATES_BOUNDING_BOX = ( + "neighborhood_vehicle_states_bounding_box" + ) + NEIGHBORHOOD_VEHICLE_STATES_HEADING = "neighborhood_vehicle_states_heading" + NEIGHBORHOOD_VEHICLE_STATES_SPEED = "neighborhood_vehicle_states_speed" + NEIGHBORHOOD_VEHICLE_STATES_ROAD_ID = "neighborhood_vehicle_states_road_id" + NEIGHBORHOOD_VEHICLE_STATES_LANE_ID = "neighborhood_vehicle_states_lane_id" + NEIGHBORHOOD_VEHICLE_STATES_LANE_INDEX = "neighborhood_vehicle_states_lane_index" + NEIGHBORHOOD_VEHICLE_STATES_LANE_POSITION = ( + "neighborhood_vehicle_states_lane_position" + ) + NEIGHBORHOOD_VEHICLE_STATES_INTEREST = "neighborhood_vehicle_states_interest" + + WAYPOINT_PATHS_POSITION = "waypoint_paths_pos" + WAYPOINT_PATHS_HEADING = "waypoint_paths_heading" + WAYPOINT_PATHS_LANE_ID = "waypoint_paths_lane_id" + WAYPOINT_PATHS_LANE_WIDTH = "waypoint_paths_lane_width" + WAYPOINT_PATHS_SPEED_LIMIT = "waypoint_paths_speed_limit" + WAYPOINT_PATHS_LANE_INDEX = "waypoint_paths_lane_index" + WAYPOINT_PATHS_LANE_OFFSET = "waypoint_paths_lane_offset" + + DISTANCE_TRAVELLED = "distance_travelled" + + ROAD_WAYPOINTS_POSITION = "road_waypoints_lanes_pos" + ROAD_WAYPOINTS_HEADING = "road_waypoints_lanes_heading" + ROAD_WAYPOINTS_LANE_ID = "road_waypoints_lanes_lane_id" + ROAD_WAYPOINTS_LANE_WIDTH = "road_waypoints_lanes_width" + ROAD_WAYPOINTS_SPEED_LIMIT = "road_waypoints_lanes_speed_limit" + ROAD_WAYPOINTS_LANE_INDEX = "road_waypoints_lanes_lane_index" + ROAD_WAYPOINTS_LANE_OFFSET = "road_waypoints_lanes_lane_offset" + + VIA_DATA_NEAR_VIA_POINTS_POSITION = "via_data_near_via_points_position" + VIA_DATA_NEAR_VIA_POINTS_LANE_INDEX = "via_data_near_via_points_lane_index" + VIA_DATA_NEAR_VIA_POINTS_ROAD_ID = "via_data_near_via_points_road_id" + VIA_DATA_NEAR_VIA_POINTS_REQUIRED_SPEED = "via_data_near_via_points_required_speed" + VIA_DATA_NEAR_VIA_POINTS_HIT = "via_data_near_via_points_hit" + + LIDAR_POINT_CLOUD_POINTS = "lidar_point_cloud_points" + LIDAR_POINT_CLOUD_HITS = "lidar_point_cloud_hits" + LIDAR_POINT_CLOUD_ORIGIN = "lidar_point_cloud_origin" + LIDAR_POINT_CLOUD_DIRECTION = "lidar_point_cloud_direction" + + VEHICLE_TYPE = "vehicle_type" + + SIGNALS_LIGHT_STATE = "signals_light_state" + SIGNALS_STOP_POINT = "signals_stop_point" + # SIGNALS_CONTROLLED_LANES = "signals_controlled_lanes" + SIGNALS_LAST_CHANGED = "signals_last_changed" diff --git a/smarts/core/signals.py b/smarts/core/signals.py index 4d2a1a3728..dbb176d35f 100644 --- a/smarts/core/signals.py +++ b/smarts/core/signals.py @@ -17,15 +17,18 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + from dataclasses import dataclass from enum import IntFlag -from typing import List, Optional +from typing import TYPE_CHECKING, List, Optional from smarts.core.colors import SceneColors from .actor import ActorState -from .coordinates import Point -from .road_map import RoadMap + +if TYPE_CHECKING: + from smarts.core import coordinates class SignalLightState(IntFlag): @@ -58,7 +61,7 @@ class SignalState(ActorState): """Traffic signal state information.""" state: Optional[SignalLightState] = None - stopping_pos: Optional[Point] = None + stopping_pos: Optional[coordinates.Point] = None controlled_lanes: Optional[List[str]] = None last_changed: Optional[float] = None # will be None if not known diff --git a/smarts/core/simulation_frame.py b/smarts/core/simulation_frame.py index 6a5c3fdb98..a5b0fab85a 100644 --- a/smarts/core/simulation_frame.py +++ b/smarts/core/simulation_frame.py @@ -19,15 +19,22 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging import re from dataclasses import dataclass from functools import cached_property, lru_cache -from typing import Any, Dict, List, Optional, Set +from typing import TYPE_CHECKING, Dict, List, Optional, Set -from smarts.core.actor import ActorState -from smarts.core.agent_interface import AgentInterface -from smarts.core.vehicle_state import Collision, VehicleState +if TYPE_CHECKING: + from smarts.core.actor import ActorState + from smarts.core.agent_interface import AgentInterface + from smarts.core.provider import ProviderState + from smarts.core.sensor import Sensor + from smarts.core.sensors import SensorState + from smarts.core.vehicle_state import Collision, VehicleState + from smarts.sstudio.sstypes import MapSpec logger = logging.getLogger(__name__) @@ -44,17 +51,17 @@ class SimulationFrame: elapsed_sim_time: float fixed_timestep: float resetting: bool - map_spec: Any + map_spec: MapSpec last_dt: float - last_provider_state: Any + last_provider_state: ProviderState step_count: int vehicle_collisions: Dict[str, List[Collision]] vehicles_for_agents: Dict[str, List[str]] vehicle_ids: Set[str] vehicle_states: Dict[str, VehicleState] - vehicle_sensors: Dict[str, Dict[str, Any]] + vehicle_sensors: Dict[str, Dict[str, Sensor]] - sensor_states: Any + sensor_states: Dict[str, SensorState] interest_filter: re.Pattern # TODO MTA: renderer can be allowed here as long as it is only type information # renderer_type: Any = None @@ -100,7 +107,7 @@ def interest_actors( return {} def actor_is_interest( - self, actor_id, extension: Optional[re.Pattern] = None + self, actor_id: str, extension: Optional[re.Pattern] = None ) -> bool: """Determine if the actor is of interest. @@ -112,7 +119,7 @@ def actor_is_interest( """ return actor_id in self.interest_actors(extension) - def vehicle_did_collide(self, vehicle_id) -> bool: + def vehicle_did_collide(self, vehicle_id: str) -> bool: """Test if the given vehicle had any collisions in the last physics update.""" vehicle_collisions = self.vehicle_collisions.get(vehicle_id, []) for c in vehicle_collisions: @@ -120,7 +127,7 @@ def vehicle_did_collide(self, vehicle_id) -> bool: return True return False - def filtered_vehicle_collisions(self, vehicle_id) -> List[Collision]: + def filtered_vehicle_collisions(self, vehicle_id: str) -> List[Collision]: """Get a list of all collisions the given vehicle was involved in during the last physics update. """ diff --git a/smarts/core/simulation_local_constants.py b/smarts/core/simulation_local_constants.py index b65d056e9c..fbf215155d 100644 --- a/smarts/core/simulation_local_constants.py +++ b/smarts/core/simulation_local_constants.py @@ -19,16 +19,20 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + from dataclasses import dataclass -from typing import Any +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from smarts.core.road_map import RoadMap # TODO MTA: Consider using EzPickle base @dataclass(frozen=True) class SimulationLocalConstants: """This is state that should only change every reset.""" - road_map: Any + road_map: RoadMap road_map_hash: int def __eq__(self, __o: object) -> bool: diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index 5bba3e618e..6395df8575 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -76,7 +76,7 @@ ProviderState, ) from .road_map import RoadMap -from .scenario import Mission, Scenario +from .scenario import NavigationMission, Scenario from .sensor_manager import SensorManager from .signal_provider import SignalProvider from .signals import SignalLightState, SignalState @@ -86,6 +86,7 @@ from .trap_manager import TrapManager from .utils import pybullet from .utils.core_math import rounder_for_dt +from .utils.custom_exceptions import RendererException from .utils.id import Id from .utils.pybullet import bullet_client as bc from .vehicle import Vehicle @@ -140,7 +141,6 @@ def __init__( ): conf = config() self._log = logging.getLogger(self.__class__.__name__) - self._log.setLevel(level=logging.ERROR) self._sim_id = Id.new("smarts") self._is_setup = False self._is_destroyed = False @@ -155,12 +155,12 @@ def __init__( visdom = None self._visdom: Any = None - if conf("visdom", "enabled", default=False, cast=bool) or visdom is True: + if conf("visdom", "enabled", cast=bool) or visdom is True: from smarts.visdom.visdom_client import VisdomClient self._visdom = VisdomClient( - hostname=conf("visdom", "hostname", default="http://localhost"), - port=conf("visdom", "port", default=8097), + hostname=conf("visdom", "hostname"), + port=conf("visdom", "port"), ) elif not isinstance(self._visdom, bool): self._visdom = visdom @@ -274,15 +274,17 @@ def step( except (KeyboardInterrupt, SystemExit): # ensure we clean-up if the user exits the simulation self._log.info("Simulation was interrupted by the user.") - if not config()("core", "debug", default=False, cast=bool): - self.destroy() + self.destroy() raise # re-raise the KeyboardInterrupt + except RendererException: + self.destroy() + raise # re-raise renderer not found exceptions without retrying. except Exception as e: self._log.error( "Simulation crashed with exception. Attempting to cleanly shutdown." ) self._log.exception(e) - if not config().get_setting("core", "debug", default=False, cast=bool): + if not config().get_setting("core", "debug", cast=bool): self.destroy() raise # re-raise @@ -345,10 +347,12 @@ def _step(self, agent_actions, time_delta_since_last_step: Optional[float] = Non # want these during their observation/reward computations. # This is a hack to give us some short term perf wins. Longer term we # need to expose better support for batched computations - self._sync_smarts_and_provider_actor_states(provider_state) - self._sensor_manager.clean_up_sensors_for_actors( - set(v.actor_id for v in self._vehicle_states), renderer=self.renderer_ref - ) + with timeit("Syncing provider state", self._log.debug): + self._sync_smarts_and_provider_actor_states(provider_state) + self._sensor_manager.clean_up_sensors_for_actors( + set(v.actor_id for v in self._vehicle_states), + renderer=self.renderer_ref, + ) # Reset frame state try: @@ -367,9 +371,12 @@ def _step(self, agent_actions, time_delta_since_last_step: Optional[float] = Non # so that all updates are ready before rendering happens per with timeit("Syncing the renderer", self._log.debug): self.renderer_ref.sync(self.cached_frame) - with timeit("Running through the render pipeline", self._log.debug): - self.renderer_ref.render() + # TODO OBSERVATIONS: this needs to happen between the base and rendered observations + # with timeit("Running through the render pipeline", self._log.debug): + # self.renderer_ref.render() + + # TODO OBSERVATIONS: Need to split the observation generation with timeit("Calculating observations and rewards", self._log.debug): observations, rewards, scores, dones = self._agent_manager.observe() @@ -459,7 +466,7 @@ def reset( - If no agents: the initial simulation observation at `start_time` - If agents: the first step of the simulation with an agent observation """ - tries = config()("core", "reset_retries", 0, cast=int) + 1 + tries = config()("core", "reset_retries", cast=int) + 1 first_exception = None for _ in range(tries): try: @@ -536,8 +543,6 @@ def setup(self, scenario: Scenario): self._check_valid() self._scenario = scenario - if self._renderer: - self._renderer.setup(scenario) self._setup_bullet_client(self._bullet_client) provider_state = self._setup_providers(self._scenario) self._vehicle_index.load_vehicle_definitions_list( @@ -548,6 +553,10 @@ def setup(self, scenario: Scenario): self._bubble_manager = BubbleManager(scenario.bubbles, scenario.road_map) for actor_capture_manager in self._actor_capture_managers: actor_capture_manager.reset(scenario, self) + if self._renderer or any( + a.requires_rendering for a in self._agent_manager.agent_interfaces.values() + ): + self.renderer.setup(scenario) self._harmonize_providers(provider_state) self._last_provider_state = provider_state @@ -595,7 +604,7 @@ def switch_ego_agents(self, agent_interfaces: Dict[str, AgentInterface]): self._is_setup = False def add_agent_with_mission( - self, agent_id: str, agent_interface: AgentInterface, mission: Mission + self, agent_id: str, agent_interface: AgentInterface, mission: NavigationMission ): """Add an agent to the simulation. The simulation will attempt to provide a vehicle for the agent. @@ -616,7 +625,7 @@ def add_agent_and_switch_control( vehicle_id: str, agent_id: str, agent_interface: AgentInterface, - mission: Mission, + mission: NavigationMission, ) -> Vehicle: """Add the new specified ego agent and then take over control of the specified vehicle.""" self._check_valid() @@ -632,7 +641,7 @@ def switch_control_to_agent( self, vehicle_id: str, agent_id: str, - mission: Mission, + mission: NavigationMission, recreate: bool, is_hijacked: bool, ) -> Vehicle: @@ -1040,21 +1049,21 @@ def observe_from( def renderer(self) -> RendererBase: """The renderer singleton. On call, the sim will attempt to create it if it does not exist.""" if not self._renderer: - from .utils.custom_exceptions import RendererException - try: + from smarts.core.renderer_base import DEBUG_MODE from smarts.p3d.renderer import Renderer - self._renderer = Renderer(self._sim_id) - except ImportError as e: - raise RendererException.required_to("use camera observations") - except Exception as e: + self._renderer = Renderer(self._sim_id, debug_mode=DEBUG_MODE.ERROR) + except ImportError as exc: + raise RendererException.required_to("use camera observations") from exc + except Exception as exc: self._renderer = None - raise RendererException("Unable to create renderer.") + raise RendererException("Unable to create renderer.") from exc if not self._renderer.is_setup: if self._scenario: self._renderer.setup(self._scenario) self._vehicle_index.begin_rendering_vehicles(self._renderer) + assert self._renderer is not None return self._renderer @property @@ -1378,39 +1387,47 @@ def _provider_actions( def _step_providers(self, actions) -> ProviderState: provider_vehicle_actions = dict() for provider in self.providers: - agent_actions, vehicle_actions = self._provider_actions(provider, actions) - provider_vehicle_actions[provider] = vehicle_actions - if isinstance(provider, AgentsProvider): - provider.perform_agent_actions(agent_actions) + with timeit( + f"Performing actions on {provider.__class__.__name__}", self._log.debug + ): + agent_actions, vehicle_actions = self._provider_actions( + provider, actions + ) + provider_vehicle_actions[provider] = vehicle_actions + if isinstance(provider, AgentsProvider): + provider.perform_agent_actions(agent_actions) - self._check_ground_plane() - self._step_pybullet() - self._process_collisions() + with timeit("Stepping physics", self._log.debug): + self._check_ground_plane() + self._step_pybullet() + self._process_collisions() accumulated_provider_state = ProviderState() agent_vehicle_ids = self._vehicle_index.agent_vehicle_ids() for provider in self.providers: - try: - provider_state = provider.step( - provider_vehicle_actions[provider], - self._last_dt, - self._elapsed_sim_time, - ) - except Exception as provider_error: - provider_state = self._handle_provider(provider, provider_error) - raise + with timeit(f"Stepping {provider.__class__.__name__}", self._log.debug): + try: + provider_state = provider.step( + provider_vehicle_actions[provider], + self._last_dt, + self._elapsed_sim_time, + ) + except Exception as provider_error: + provider_state = self._handle_provider(provider, provider_error) + raise - # by this point, "stop_managing()" should have been called for the hijacked vehicle on all TrafficProviders - assert not isinstance( - provider, TrafficProvider - ) or not provider_state.intersects( - agent_vehicle_ids - ), f"{agent_vehicle_ids} in {provider_state.actors}" + # by this point, "stop_managing()" should have been called for the hijacked vehicle on all TrafficProviders + assert not isinstance( + provider, TrafficProvider + ) or not provider_state.intersects( + agent_vehicle_ids + ), f"{agent_vehicle_ids} in {provider_state.actors}" - accumulated_provider_state.merge(provider_state) + accumulated_provider_state.merge(provider_state) - self._harmonize_providers(accumulated_provider_state) + with timeit("Harmonizing provider state", self._log.debug): + self._harmonize_providers(accumulated_provider_state) return accumulated_provider_state def _sync_smarts_and_provider_actor_states( diff --git a/smarts/core/sumo_road_network.py b/smarts/core/sumo_road_network.py index a0700acfdc..d74c16a1e0 100644 --- a/smarts/core/sumo_road_network.py +++ b/smarts/core/sumo_road_network.py @@ -546,7 +546,7 @@ def length(self) -> float: length = 0 shape = self._sumo_lane.getShape() for p1, p2 in zip(shape, shape[1:]): - length += np.linalg.norm(np.array(p2) - np.array(p1)) + length += np.linalg.norm(np.subtract(p2, p1)) return length @cached_property @@ -793,7 +793,7 @@ def from_lane_coord(self, lane_point: RefLinePoint) -> Point: ) dx = dv * (x2 - x) dy = dv * (y2 - y) - dd = lane_point.t / np.linalg.norm((dx, dy)) + dd = (lane_point.t or 0) / np.linalg.norm((dx, dy)) x -= dy * dd y += dx * dd return Point(x=x, y=y) @@ -1248,7 +1248,7 @@ def waypoint_paths( def _angle_between(pose, wp): heading_vec = pose.heading.direction_vector() - wp_vec = wp.pos - pose.position[:2] + wp_vec = np.subtract(wp.pos, pose.position[:2]) angle = np.arccos( np.dot(heading_vec, wp_vec) / (np.linalg.norm(heading_vec) * np.linalg.norm(wp_vec)) @@ -1263,7 +1263,8 @@ def _angle_between(pose, wp): # Only include paths that start in the junction, or are close to the vehicle if ( self.lane_by_id(path[0].lane_id).in_junction - or np.linalg.norm(path[0].pos - pose.position[:2]) < 7.0 + or np.linalg.norm(np.subtract(path[0].pos, pose.position[:2])) + < 7.0 ): waypoint_paths.append(path) diff --git a/smarts/core/sumo_traffic_simulation.py b/smarts/core/sumo_traffic_simulation.py index 85596d26c3..1cac65c3b4 100644 --- a/smarts/core/sumo_traffic_simulation.py +++ b/smarts/core/sumo_traffic_simulation.py @@ -17,13 +17,14 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations import logging import random import weakref from functools import partial from pathlib import Path -from typing import Final, Iterable, List, Optional, Sequence, Tuple +from typing import TYPE_CHECKING, Final, Iterable, List, Optional, Sequence, Tuple import numpy as np from shapely.affinity import rotate as shapely_rotate @@ -61,6 +62,9 @@ import traci.constants as tc # isort:skip +if TYPE_CHECKING: + import smarts.core.scenario + class SumoTrafficSimulation(TrafficProvider): """ @@ -85,7 +89,7 @@ class SumoTrafficSimulation(TrafficProvider): Remove only agent vehicles used by SMARTS and not delete other SUMO vehicles when the traffic simulation calls to tear-down traci_retries: - The number of times to retry acquisition of a TraCI server before erroring. + The number of times to retry acquisition of a TraCI server before throwing an exception. """ _HAS_DYNAMIC_ATTRIBUTES = True @@ -504,7 +508,10 @@ def reset(self): pass def recover( - self, scenario, elapsed_sim_time: float, error: Optional[Exception] = None + self, + scenario: smarts.core.scenario.Scenario, + elapsed_sim_time: float, + error: Optional[Exception] = None, ) -> Tuple[ProviderState, bool]: if isinstance(error, self._traci_exceptions): self._handle_traci_exception(error) diff --git a/smarts/core/tests/test_dynamics_backend.py b/smarts/core/tests/test_dynamics_backend.py index 4cfc35335e..efbaad6fcf 100644 --- a/smarts/core/tests/test_dynamics_backend.py +++ b/smarts/core/tests/test_dynamics_backend.py @@ -69,5 +69,5 @@ def test_set_pose(chassis): chassis.set_pose(Pose.from_center([137, -5.8, 1], Heading(1.8))) position, heading = chassis.pose.position, chassis.pose.heading - assert np.linalg.norm(position - np.array([137, -5.8, 1])) < 1e-16 + assert np.linalg.norm(np.subtract(position, np.array([137, -5.8, 1]))) < 1e-16 assert np.isclose(heading, 1.8) diff --git a/smarts/core/tests/test_motion_planner_provider.py b/smarts/core/tests/test_motion_planner_provider.py index fcb3db601a..805eb07f10 100644 --- a/smarts/core/tests/test_motion_planner_provider.py +++ b/smarts/core/tests/test_motion_planner_provider.py @@ -26,7 +26,7 @@ from smarts.core.agent_interface import ActionSpaceType, AgentInterface, DoneCriteria from smarts.core.coordinates import Heading -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS @@ -48,7 +48,7 @@ def smarts(): @pytest.fixture def loop_scenario(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((0, 0, 0.5)), Heading(0)), goal=EndlessGoal(), ) @@ -71,5 +71,5 @@ def test_we_reach_target_pose_at_given_time(smarts, loop_scenario): observations, _, dones, _ = smarts.step(actions) ego_state = observations[AGENT_ID].ego_vehicle_state - assert np.linalg.norm(ego_state.position[:2] - np.array(target_position)) < 1e-16 + assert np.linalg.norm(np.subtract(ego_state.position[:2], target_position)) < 1e-16 assert np.isclose(ego_state.heading, target_heading) diff --git a/smarts/core/tests/test_observations.py b/smarts/core/tests/test_observations.py index e8af051434..396b1b5396 100644 --- a/smarts/core/tests/test_observations.py +++ b/smarts/core/tests/test_observations.py @@ -35,6 +35,7 @@ AgentInterface, DrivableAreaGridMap, NeighborhoodVehicles, + OcclusionMap, RoadWaypoints, Signals, ) @@ -42,13 +43,13 @@ from smarts.core.controllers import ActionSpaceType from smarts.core.coordinates import Heading, Point from smarts.core.observations import DrivableAreaGridMap as ObsDrivableAreaGridMap -from smarts.core.observations import ( - GridMapMetadata, - Observation, - OccupancyGridMap, - TopDownRGB, +from smarts.core.observations import GridMapMetadata, Observation, TopDownRGB +from smarts.core.plan import ( + NavigationMission, + PositionalGoal, + Start, + default_entry_tactic, ) -from smarts.core.plan import Mission, PositionalGoal, Start, default_entry_tactic from smarts.core.scenario import Scenario from smarts.core.signals import SignalLightState from smarts.core.smarts import SMARTS @@ -60,7 +61,7 @@ AGENT_ID = "Agent-007" -NUM_STEPS = 30 +NUM_STEPS = 20 MAP_WIDTH = 1536 MAP_HEIGHT = 1536 HALF_WIDTH = MAP_WIDTH / 2 @@ -84,6 +85,9 @@ def agent_interface(): occupancy_grid_map=OGM( width=MAP_WIDTH, height=MAP_HEIGHT, resolution=MAP_RESOLUTION ), + occlusion_map=OcclusionMap( + width=MAP_WIDTH, height=MAP_HEIGHT, resolution=MAP_RESOLUTION + ), top_down_rgb=RGB(width=MAP_WIDTH, height=MAP_HEIGHT, resolution=MAP_RESOLUTION), action=ActionSpaceType.Lane, signals=Signals(100.0), @@ -157,12 +161,11 @@ def apply_tolerance(arr, x, y, tolerance): def sample_vehicle_pos( lens, rgb: TopDownRGB, - ogm: OccupancyGridMap, drivable_area: ObsDrivableAreaGridMap, vehicle_pos, ): rgb_x, rgb_y = project_2d(lens, rgb.metadata, vehicle_pos) - ogm_x, ogm_y = project_2d(lens, ogm.metadata, vehicle_pos) + # ogm_x, ogm_y = project_2d(lens, occ.metadata, vehicle_pos) drivable_area_x, drivable_area_y = project_2d( lens, drivable_area.metadata, vehicle_pos ) @@ -175,7 +178,8 @@ def sample_vehicle_pos( ) # OGM - assert np.count_nonzero(apply_tolerance(ogm.data, ogm_x, ogm_y, tolerance)) + # print(np.count_nonzero(apply_tolerance(ogm.data, ogm_x, ogm_y, tolerance))) + # assert np.count_nonzero(apply_tolerance(occ.data, ogm_x, ogm_y, tolerance)) # Check if vehicles are within drivable area # Drivable area grid map @@ -197,8 +201,8 @@ def test_observations(env, agent_spec): # RGB rgb = observations[AGENT_ID].top_down_rgb - # OGM - ogm = observations[AGENT_ID].occupancy_grid_map + # OCC + occ = observations[AGENT_ID].occlusion_map # Drivable area drivable_area = observations[AGENT_ID].drivable_area_grid_map @@ -211,7 +215,6 @@ def test_observations(env, agent_spec): sample_vehicle_pos( lens, rgb, - ogm, drivable_area, ego_vehicle_position, ) @@ -221,7 +224,6 @@ def test_observations(env, agent_spec): sample_vehicle_pos( lens, rgb, - ogm, drivable_area, neighbor_vehicle.position, ) @@ -267,7 +269,7 @@ def test_observations(env, agent_spec): @pytest.fixture def scenario(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((20.40, 68.40)), Heading(-0.5 * math.pi)), goal=PositionalGoal(Point(128.40, 0), 10), entry_tactic=default_entry_tactic(1.0), diff --git a/smarts/core/tests/test_parallel_sensors.py b/smarts/core/tests/test_parallel_sensors.py index f1681d9b19..156f9f2a8a 100644 --- a/smarts/core/tests/test_parallel_sensors.py +++ b/smarts/core/tests/test_parallel_sensors.py @@ -25,7 +25,7 @@ from smarts.core.agent_interface import AgentInterface, AgentType from smarts.core.controllers import ActionSpaceType -from smarts.core.plan import Mission +from smarts.core.plan import NavigationMission from smarts.core.scenario import Scenario from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver from smarts.core.sensors.parallel_sensor_resolver import ParallelSensorResolver @@ -54,7 +54,7 @@ def scenario() -> Scenario: ), ) missions = [ - Mission.random_endless_mission( + NavigationMission.random_endless_mission( s.road_map, ) for _ in AGENT_IDS diff --git a/smarts/core/tests/test_renderers.py b/smarts/core/tests/test_renderers.py index ac8bf24dce..bdf8f48573 100644 --- a/smarts/core/tests/test_renderers.py +++ b/smarts/core/tests/test_renderers.py @@ -19,12 +19,14 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import importlib.resources as pkg_resources import math -import threading +from typing import Optional, Set import numpy as np import pytest +import smarts.core.glsl from smarts.core.agent_interface import ( ActionSpaceType, AgentInterface, @@ -33,11 +35,18 @@ ) from smarts.core.colors import SceneColors from smarts.core.coordinates import Heading, Pose -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start +from smarts.core.renderer_base import ( + RendererBase, + RendererNotSetUpWarning, + ShaderStepBufferDependency, +) from smarts.core.scenario import Scenario +from smarts.core.shader_buffer import BufferID from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation from smarts.core.utils.custom_exceptions import RendererException +from smarts.core.utils.tests.fixtures import large_observation AGENT_ID = "Agent-007" @@ -52,7 +61,7 @@ def _smarts_with_agent(agent) -> SMARTS: @pytest.fixture -def smarts(): +def smarts_w_renderer(): buddha = AgentInterface( debug=True, done_criteria=DoneCriteria(collision=False, off_road=False, off_route=False), @@ -82,7 +91,7 @@ def smarts_wo_renderer(): @pytest.fixture def scenario(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), ) @@ -94,20 +103,41 @@ def scenario(): return scenario -def test_optional_renderer(smarts: SMARTS, scenario): - assert not smarts.is_rendering - renderer = smarts.renderer +@pytest.fixture(params=["p3d"], scope="module") +def renderer(request: pytest.FixtureRequest): + renderer: Optional[RendererBase] = None + + if request.param == "p3d": + from smarts.p3d.renderer import BACKEND_LITERALS, DEBUG_MODE, Renderer + + renderer = Renderer("na", debug_mode=DEBUG_MODE.WARNING) + + assert renderer is not None + yield renderer + + renderer.destroy() + + +@pytest.fixture +def observation_buffers() -> Set[BufferID]: + return {v for v in BufferID} + + +def test_optional_renderer(smarts_w_renderer: SMARTS, scenario: Scenario): + assert not smarts_w_renderer.is_rendering + renderer = smarts_w_renderer.renderer assert renderer - smarts._renderer = None - smarts.reset(scenario) - assert smarts.is_rendering + smarts_w_renderer._renderer = None + smarts_w_renderer.reset(scenario) + assert smarts_w_renderer.is_rendering - smarts._renderer = None - for _ in range(10): - smarts.step({AGENT_ID: "keep_lane"}) + smarts_w_renderer._renderer = None + + with pytest.raises(expected_exception=AttributeError, match=r"NoneType"): + smarts_w_renderer.step({AGENT_ID: "keep_lane"}) - assert not smarts.is_rendering + assert not smarts_w_renderer.is_rendering def test_no_renderer(smarts_wo_renderer: SMARTS, scenario): @@ -118,3 +148,51 @@ def test_no_renderer(smarts_wo_renderer: SMARTS, scenario): smarts_wo_renderer.step({AGENT_ID: "keep_lane"}) assert not smarts_wo_renderer.is_rendering + + +def test_custom_shader_pass_buffers( + renderer: Optional[RendererBase], + observation_buffers: Set[BufferID], + large_observation, +): + # Use a full dummy observation. + # Construct the renderer + # Construct the shader pass + # Use a shader that uses all buffers + # Assign all shader buffers in the pass. + # Update the shader pass using the current observation + # Render + # Test that the pixels default to white. (black is error) + + assert renderer + renderer.reset() + camera_id = "test_shader" + with pkg_resources.path( + smarts.core.glsl, "test_custom_shader_pass_shader.frag" + ) as fshader: + renderer.build_shader_step( + camera_id, + fshader_path=fshader, + dependencies=[ + ShaderStepBufferDependency( + buffer_id=b_id, script_uniform_name=b_id.value + ) + for b_id in observation_buffers + ], + priority=40, + height=100, + width=100, + ) + + camera = renderer.camera_for_id(camera_id=camera_id) + camera.update(Pose.from_center(np.array([0, 0, 0]), Heading(0)), 10) + camera.update(observation=large_observation) + + with pytest.warns(RendererNotSetUpWarning): + renderer.render() + + ram_image = camera.wait_for_ram_image("RGB") + mem_view = memoryview(ram_image) + image: np.ndarray = np.frombuffer(mem_view, np.uint8)[::3] + + assert image[0] == 0 diff --git a/smarts/core/tests/test_sensor_worker.py b/smarts/core/tests/test_sensor_worker.py index a3619cc69c..99ff30d994 100644 --- a/smarts/core/tests/test_sensor_worker.py +++ b/smarts/core/tests/test_sensor_worker.py @@ -25,7 +25,7 @@ from smarts.core.agent_interface import AgentInterface, AgentType from smarts.core.controllers import ActionSpaceType -from smarts.core.plan import Mission +from smarts.core.plan import NavigationMission from smarts.core.scenario import Scenario from smarts.core.sensors import Observation from smarts.core.sensors.parallel_sensor_resolver import ( @@ -60,7 +60,7 @@ def scenario() -> Scenario: ), ) missions = [ - Mission.random_endless_mission( + NavigationMission.random_endless_mission( s.road_map, ) for _ in AGENT_IDS diff --git a/smarts/core/tests/test_sensors.py b/smarts/core/tests/test_sensors.py index 4cbe14d57e..3f382b67ba 100644 --- a/smarts/core/tests/test_sensors.py +++ b/smarts/core/tests/test_sensors.py @@ -19,17 +19,37 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import importlib.resources as pkg_resources +import math +import warnings +from functools import partial from unittest import mock import numpy as np import pytest from helpers.scenario import temp_scenario -from smarts.core.coordinates import Heading, Pose +from smarts.core import glsl +from smarts.core.agent_interface import ( + CustomRenderCameraDependency, + CustomRenderVariableDependency, +) +from smarts.core.coordinates import Heading, Pose, RefLinePoint +from smarts.core.observations import CustomRenderData from smarts.core.plan import Plan from smarts.core.scenario import Scenario -from smarts.core.sensors import DrivenPathSensor, TripMeterSensor, WaypointsSensor -from smarts.core.utils.core_math import squared_dist +from smarts.core.sensor import ( + CustomRenderSensor, + LanePositionSensor, + RoadWaypointsSensor, +) +from smarts.core.sensors import ( + AccelerometerSensor, + DrivenPathSensor, + TripMeterSensor, + ViaSensor, + WaypointsSensor, +) from smarts.sstudio import gen_scenario from smarts.sstudio import sstypes as t @@ -62,7 +82,7 @@ def test_driven_path_sensor(): @pytest.fixture -def scenarios(): +def scenario(): with temp_scenario(name="straight", map="maps/6lane.net.xml") as scenario_root: ego_missions = [ t.Mission( @@ -77,13 +97,12 @@ def scenarios(): output_dir=scenario_root, ) - yield Scenario.variations_for_all_scenario_roots( - [str(scenario_root)], [AGENT_ID] + return next( + Scenario.variations_for_all_scenario_roots([str(scenario_root)], [AGENT_ID]) ) -def test_trip_meter_sensor(scenarios): - scenario: Scenario = next(scenarios) +def test_trip_meter_sensor(scenario): sim = mock.Mock() vehicle_state = mock.Mock() @@ -123,8 +142,7 @@ def test_trip_meter_sensor(scenarios): sensor.teardown() -def test_waypoints_sensor(scenarios): - scenario = next(scenarios) +def test_waypoints_sensor(scenario): vehicle_state = mock.Mock() vehicle_state.pose = Pose( position=np.array([33, -65, 0]), @@ -141,3 +159,154 @@ def test_waypoints_sensor(scenarios): assert len(waypoints) == 3 sensor.teardown() + + +def test_road_waypoints_sensor(scenario): + assert isinstance(scenario, Scenario) + assert scenario.road_map.map_spec + assert scenario.road_map.map_spec.lanepoint_spacing == 1.0 + + vehicle_state = mock.Mock() + vehicle_state.pose = Pose( + position=np.array([33, -65, 0]), + orientation=np.array([0, 0, 0, 0]), + heading_=Heading(0), + ) + + mission = scenario.missions[AGENT_ID] + plan = Plan(scenario.road_map, mission) + + sensor = RoadWaypointsSensor(4) + road_waypoints = sensor(vehicle_state, plan, scenario.road_map) + + expected_lanes = { + "edge-west-WE_0", + "edge-west-WE_1", + "edge-west-WE_2", + "edge-west-EW_0", + "edge-west-EW_1", + "edge-west-EW_2", + } + + assert len(road_waypoints) == 1 + + lanes = road_waypoints[-1] + assert len(expected_lanes ^ lanes.keys()) == 0 + assert len(lanes["edge-west-WE_0"][-1]) == 9 + + sensor.teardown() + + +def test_accelerometer_sensor(): + dt = 0.1 + v_per_step = 15.0 + sensor = AccelerometerSensor() + + def _get_next(step, a_sensor): + l_velocity = np.array((0, v_per_step * dt * step)) + a_velocity = np.array((0, v_per_step * dt * step)) + return sensor(l_velocity, a_velocity, dt=0.1) + + step = 1 + (linear_acc, angular_acc, linear_jerk, angular_jerk) = _get_next(step, sensor) + + assert np.all(linear_acc == np.array([0.0, 0.0, 0.0])) + assert np.all(angular_acc == np.array([0.0, 0.0, 0.0])) + assert np.all(linear_jerk == np.array([0.0, 0.0, 0.0])) + assert np.all(angular_jerk == np.array([0.0, 0.0, 0.0])) + + step = 2 + (linear_acc, angular_acc, linear_jerk, angular_jerk) = _get_next(step, sensor) + + assert np.all(linear_acc == (0, v_per_step)) + assert np.all(angular_acc == (0, v_per_step)) + assert np.all(linear_jerk == np.array([0.0, 0.0, 0.0])) + assert np.all(angular_jerk == np.array([0.0, 0.0, 0.0])) + + step = 3 + (linear_acc, angular_acc, linear_jerk, angular_jerk) = _get_next(step, sensor) + + assert np.all(linear_acc == (0, v_per_step)) + assert np.all(angular_acc == (0, v_per_step)) + assert np.all(linear_jerk == (0, 0)) + assert np.all(angular_jerk == (0, 0)) + + sensor.teardown() + + +def test_lane_position_sensor(scenario): + sensor = LanePositionSensor() + + vehicle_state = mock.Mock() + vehicle_state.actor_id = "dummy" + vehicle_state.pose = Pose( + position=np.array([143.0, -11, 0.1]), + orientation=np.array([0, 0, 0, 0]), + heading_=Heading(0), + ) + + off = sensor(scenario.road_map.lane_by_id("edge-north-NS_0"), vehicle_state) + + assert off == RefLinePoint(20.999999999999996, 1.004987562112089, 0) + sensor.teardown() + + +def test_signals_sensor(): + pass + + +def test_custom_render_sensor(): + from smarts.p3d.renderer import Renderer + + renderer = Renderer("R1") + + vehicle_state = mock.Mock() + vehicle_state.actor_id = "dummy" + vehicle_state.pose = Pose( + position=np.array([33, -65, 0]), + orientation=np.array([0, 0, 0, 0]), + heading_=Heading(0), + ) + + with pkg_resources.path(glsl, "simplex_shader.frag") as frag_shader: + + sensor_gen = partial( + CustomRenderSensor, + vehicle_state=vehicle_state, + width=256, + height=256, + resolution=1, + fragment_shader_path=frag_shader, + renderer=renderer, + render_dependencies=( + CustomRenderVariableDependency( + value=1.0, + variable_name="scale", + ), + ), + ogm_sensor=None, + top_down_rgb_sensor=None, + drivable_area_grid_map_sensor=None, + occlusion_map_sensor=None, + ) + sensor = sensor_gen(name="simplex") + sensor2 = sensor_gen( + name="2nd", + render_dependencies=( + CustomRenderCameraDependency( + camera_dependency_name=sensor.name, variable_name="iChannel0" + ), + CustomRenderVariableDependency( + value=10.0, + variable_name="scale", + ), + ), + ) + + renderer.render() + + image: CustomRenderData = sensor2(renderer) + assert image.data.shape == (256, 256, 3) + + sensor.teardown() + renderer.destroy() diff --git a/smarts/core/tests/test_smarts.py b/smarts/core/tests/test_smarts.py index e2a7cdb4cc..b3d7396744 100644 --- a/smarts/core/tests/test_smarts.py +++ b/smarts/core/tests/test_smarts.py @@ -31,7 +31,7 @@ NeighborhoodVehicles, ) from smarts.core.coordinates import Heading -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation @@ -40,7 +40,7 @@ @pytest.fixture def scenarios(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), ) diff --git a/smarts/core/tests/test_smarts_reset.py b/smarts/core/tests/test_smarts_reset.py index 0918ae7e6c..290d63ecf1 100644 --- a/smarts/core/tests/test_smarts_reset.py +++ b/smarts/core/tests/test_smarts_reset.py @@ -28,7 +28,7 @@ from smarts.core import seed from smarts.core.agent_interface import ActionSpaceType, AgentInterface from smarts.core.coordinates import Heading -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation @@ -36,7 +36,7 @@ @pytest.fixture def scenarios(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), ) diff --git a/smarts/core/tests/test_traffic_simulation.py b/smarts/core/tests/test_traffic_simulation.py index 2f5dc9b54d..f73ababdae 100644 --- a/smarts/core/tests/test_traffic_simulation.py +++ b/smarts/core/tests/test_traffic_simulation.py @@ -29,7 +29,7 @@ from smarts.core.agent_interface import ActionSpaceType, AgentInterface from smarts.core.coordinates import Heading from smarts.core.local_traffic_provider import LocalTrafficProvider -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation @@ -40,7 +40,7 @@ @pytest.fixture def scenarios(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), ) diff --git a/smarts/core/tests/test_trajectory_interpolation_provider.py b/smarts/core/tests/test_trajectory_interpolation_provider.py index 55dbd60741..42dcd4cc0a 100644 --- a/smarts/core/tests/test_trajectory_interpolation_provider.py +++ b/smarts/core/tests/test_trajectory_interpolation_provider.py @@ -184,10 +184,10 @@ def test_trajectory_interpolation_controller(controller_actions, bullet_client): if "error" in vehicle_id: assert has_error elif vehicle_id == "budda": - assert np.linalg.norm(new_pos - original_position) < 1e-16 + assert np.linalg.norm(np.subtract(new_pos, original_position)) < 1e-16 assert np.isclose(vehicle.heading, original_heading) else: - assert not np.linalg.norm(new_pos - original_position) < 1e-16 + assert np.linalg.norm(np.subtract(new_pos, original_position)) >= 1e-16 assert not np.isclose(vehicle.heading, original_heading) @@ -271,6 +271,6 @@ def test_trajectory_interpolation_provider(smarts, agent_spec, scenario): init_position = init_ego_state.position init_heading = init_ego_state.heading init_speed = init_ego_state.speed - assert np.linalg.norm(curr_position[:2] - init_position[:2]) > 1e-16 + assert np.linalg.norm(np.subtract(curr_position[:2], init_position[:2])) > 1e-16 assert not np.isclose(curr_heading, init_heading) assert not np.isclose(curr_speed, init_speed) diff --git a/smarts/core/trap_manager.py b/smarts/core/trap_manager.py index 0a00799ff9..48067e3529 100644 --- a/smarts/core/trap_manager.py +++ b/smarts/core/trap_manager.py @@ -17,23 +17,30 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import logging import math from collections import defaultdict from dataclasses import dataclass -from typing import Dict, List, Optional, Set, Tuple +from typing import TYPE_CHECKING, Dict, List, Optional, Set, Tuple from shapely.geometry import Polygon from smarts.core.actor_capture_manager import ActorCaptureManager from smarts.core.condition_state import ConditionState from smarts.core.coordinates import Point as MapPoint -from smarts.core.plan import Mission, Plan, Start, default_entry_tactic +from smarts.core.plan import NavigationMission, Plan, Start, default_entry_tactic from smarts.core.utils.core_math import clip, squared_dist from smarts.core.utils.file import replace from smarts.core.vehicle import Vehicle from smarts.sstudio.sstypes import MapZone, PositionalZone, TrapEntryTactic +if TYPE_CHECKING: + import smarts.core.scenario + from smarts.core.road_map import RoadMap + from smarts.core.smarts import SMARTS + @dataclass class Trap: @@ -41,7 +48,7 @@ class Trap: geometry: Polygon """The trap area within which actors are considered for capture.""" - mission: Mission + mission: NavigationMission """The mission that this trap should assign the captured actor.""" activation_time: float """The amount of time left until this trap activates.""" @@ -76,7 +83,7 @@ class _CaptureState: ready_state: ConditionState trap: Optional[Trap] vehicle_id: Optional[str] = None - updated_mission: Optional[Mission] = None + updated_mission: Optional[NavigationMission] = None default: bool = False @@ -87,11 +94,8 @@ def __init__(self): self._log = logging.getLogger(self.__class__.__name__) self._traps: Dict[str, Trap] = {} - def init_traps(self, road_map, missions, sim): + def init_traps(self, road_map, missions, sim: SMARTS): """Set up the traps used to capture actors.""" - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) self._traps.clear() cancelled_agents: Set[str] = set() for agent_id, mission in missions.items(): @@ -106,8 +110,8 @@ def init_traps(self, road_map, missions, sim): def add_trap_for_agent( self, agent_id: str, - mission: Mission, - road_map, + mission: NavigationMission, + road_map: RoadMap, sim_time: float, reject_expired: bool = False, ) -> Tuple[bool, bool]: @@ -116,7 +120,7 @@ def add_trap_for_agent( :param agent_id: The agent to associate to this trap. :type agent_id: str :param mission: The mission to assign to the agent and vehicle. - :type mission: smarts.core.plan.Mission + :type mission: smarts.core.plan.NavigationMission :param road_map: The road map to provide information to about the map. :type road_map: RoadMap :param sim_time: The current simulator time. @@ -128,7 +132,7 @@ def add_trap_for_agent( :rtype: Tuple[bool, bool] """ if mission is None: - mission = Mission.random_endless_mission(road_map) + mission = NavigationMission.random_endless_mission(road_map) if not mission.entry_tactic: mission = replace(mission, entry_tactic=default_entry_tactic()) @@ -169,11 +173,8 @@ def reset_traps(self, used_traps): ) self.remove_traps(used_traps) - def step(self, sim): + def step(self, sim: SMARTS): """Run vehicle hijacking and update agent and actor states.""" - from smarts.core.smarts import SMARTS - - assert isinstance(sim, SMARTS) capture_by_agent_id: Dict[str, _CaptureState] = defaultdict( lambda: _CaptureState( ready_state=ConditionState.FALSE, @@ -339,13 +340,24 @@ def traps(self) -> Dict[str, Trap]: """The traps in this manager.""" return self._traps - def reset(self, scenario, sim): + def reset(self, scenario: smarts.core.scenario.Scenario, sim: SMARTS): + """ + :param scenario: The scenario to initialize from. + :type scenario: smarts.core.scenario.Scenario + :param sim: The simulation this is associated to. + :type scenario: smarts.core.smarts.SMARTS + """ self.init_traps(scenario.road_map, scenario.missions, sim) def teardown(self): self._traps.clear() - def _mission2trap(self, road_map, mission: Mission, default_zone_dist: float = 6.0): + def _mission2trap( + self, + road_map: RoadMap, + mission: NavigationMission, + default_zone_dist: float = 6.0, + ): if not (hasattr(mission, "start") and hasattr(mission, "goal")): raise ValueError(f"Value {mission} is not a mission!") diff --git a/smarts/core/utils/adapters/ego_centric_adapters.py b/smarts/core/utils/adapters/ego_centric_adapters.py index f10b0bc1d1..2ab0f1be9b 100644 --- a/smarts/core/utils/adapters/ego_centric_adapters.py +++ b/smarts/core/utils/adapters/ego_centric_adapters.py @@ -55,7 +55,7 @@ def adjust_heading(h): wpps = obs.waypoint_paths or [] def _replace_via(via: Union[Via, ViaPoint]): - return _replace(via, position=transform(via.position)) + return _replace(via, position=transform(via.position + (0.0,))) vd = None if obs.via_data: @@ -63,7 +63,6 @@ def _replace_via(via: Union[Via, ViaPoint]): vd = _replace( obs.via_data, near_via_points=rpvp(obs.via_data.near_via_points), - hit_via_points=rpvp(obs.via_data.hit_via_points), ) replace_wps = lambda lwps: [ [ diff --git a/smarts/core/utils/core_math.py b/smarts/core/utils/core_math.py index b6abc3affb..44ef7d7f47 100644 --- a/smarts/core/utils/core_math.py +++ b/smarts/core/utils/core_math.py @@ -21,7 +21,7 @@ from dataclasses import dataclass from itertools import chain, permutations, product, repeat from math import factorial -from typing import Callable, List, Sequence, Tuple, Union +from typing import Callable, Generator, List, Sequence, Tuple, Union @dataclass(frozen=True) @@ -220,6 +220,20 @@ def lerp(a, b, p): return a * (1.0 - p) + b * p +def safe_division(n: float, d: float, default=math.inf): + """This method uses a short circuit form where `and` converts right side to true|false (as 1|0) + + .. note:: + + The cases are: + 1) True and == + 2) False and ``NaN`` == False + """ + if n == 0: + return 0 + return d and n / d or default + + def low_pass_filter( input_value, previous_filter_state, @@ -539,8 +553,6 @@ def welford() -> Tuple[ Tuple[ Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int] ]: Callable functions to update, get mean, get std, and get steps. """ - import math - n = 0 # steps M = 0 S = 0 @@ -652,3 +664,22 @@ def combination_pairs_with_unique_indices( if len_first <= len_second: return _unique_element_combination(first_group, second_group) return [] + + +def slope(horizontal, vertical, default=math.inf): + """Safely converts a 2 dimensional direction vector to a slope as ``y/x``. + + Args: + horizontal (float): The x growth rate. + vertical (float): The y growth rate. + default (float): The default if the value approaches infinity. Defaults to ``1e10``. + + Returns: + float: The slope of the direction vector. + """ + return safe_division(vertical, horizontal, default=default) + + +def is_power_of_2(value: int) -> bool: + """Test if the given value is a power of 2 greater than 2**0. (e.g. 2**4)""" + return (value & (value - 1) == 0) and value != 0 diff --git a/smarts/core/utils/custom_exceptions.py b/smarts/core/utils/custom_exceptions.py index 1717350f72..a769accec0 100644 --- a/smarts/core/utils/custom_exceptions.py +++ b/smarts/core/utils/custom_exceptions.py @@ -26,7 +26,7 @@ class RendererException(Exception): def required_to(cls, thing: str) -> "RendererException": """Generate a `RenderException` requiring a render to do `thing`.""" return cls( - f"""A renderer is required to {thing}. You may not have installed the [camera_obs] dependencies required to render the camera sensor observations. Install them first using the command `pip install -e .[camera_obs]` at the source directory.""" + f"""A renderer is required to {thing}. You may not have installed the [camera-obs] dependencies required to render the camera sensor observations. Install them first using the command `pip install -e .[camera-obs]` at the source directory.""" ) diff --git a/smarts/core/utils/dummy.py b/smarts/core/utils/dummy.py index 071ec13f09..56d1fec8ab 100644 --- a/smarts/core/utils/dummy.py +++ b/smarts/core/utils/dummy.py @@ -26,7 +26,7 @@ import numpy as np import smarts.sstudio.sstypes as t -from smarts.core.coordinates import Dimensions, Heading, RefLinePoint +from smarts.core.coordinates import Dimensions, Heading, Point, RefLinePoint from smarts.core.events import Events from smarts.core.observations import ( DrivableAreaGridMap, @@ -35,12 +35,16 @@ Observation, OccupancyGridMap, RoadWaypoints, + SignalObservation, TopDownRGB, VehicleObservation, + ViaPoint, Vias, ) -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.road_map import Waypoint +from smarts.core.signals import SignalLightState +from smarts.core.vehicle_state import Collision def dummy_observation() -> Observation: @@ -50,7 +54,7 @@ def dummy_observation() -> Observation: step_count=1, elapsed_sim_time=0.2, events=Events( - collisions=[], + collisions=(Collision("v", "2"),), off_road=False, off_route=False, on_shoulder=False, @@ -63,7 +67,7 @@ def dummy_observation() -> Observation: ), ego_vehicle_state=EgoVehicleObservation( id="AGENT-007-07a0ca6e", - position=np.array([161.23485529, 3.2, 0.0]), + position=(161.23485529, 3.2, 0.0), bounding_box=Dimensions(length=3.68, width=1.47, height=1.0), heading=Heading(-1.5707963267948966), speed=5.0, @@ -73,7 +77,7 @@ def dummy_observation() -> Observation: lane_id="east_2", lane_index=2, lane_position=RefLinePoint(161.23485529, 0.0, 0.0), - mission=Mission( + mission=NavigationMission( start=Start( position=np.array([163.07485529, 3.2]), heading=Heading(-1.5707963267948966), @@ -91,15 +95,15 @@ def dummy_observation() -> Observation: via=(), vehicle_spec=None, ), - linear_velocity=np.array([5.000000e00, 3.061617e-16, 0.000000e00]), - angular_velocity=np.array([0.0, 0.0, 0.0]), - linear_acceleration=np.array([0.0, 0.0, 0.0]), - angular_acceleration=np.array([0.0, 0.0, 0.0]), - linear_jerk=np.array([0.0, 0.0, 0.0]), - angular_jerk=np.array([0.0, 0.0, 0.0]), + linear_velocity=(5.000000e00, 3.061617e-16, 0.000000e00), + angular_velocity=(0.0, 0.0, 0.0), + linear_acceleration=(0.0, 0.0, 0.0), + angular_acceleration=(0.0, 0.0, 0.0), + linear_jerk=(0.0, 0.0, 0.0), + angular_jerk=(0.0, 0.0, 0.0), ), under_this_agent_control=True, - neighborhood_vehicle_states=[ + neighborhood_vehicle_states=( VehicleObservation( id="car-west_0_0-east_0_max-784511-726648-0-0.0", position=(-1.33354215, -3.2, 0.0), @@ -122,7 +126,7 @@ def dummy_observation() -> Observation: lane_index=1, lane_position=RefLinePoint(-1.47159011, 0.0, 0.0), ), - ], + ), waypoint_paths=[ [ Waypoint( @@ -316,6 +320,11 @@ def dummy_observation() -> Observation: ], } ), - via_data=Vias(near_via_points=[], hit_via_points=[]), + signals=( + SignalObservation( + SignalLightState.GO, Point(181.0, 0.0), ("east_1",), None + ), + ), + via_data=Vias(near_via_points=(ViaPoint((181.0, 0.0), 1, "east", 5.0, False),)), steps_completed=4, ) diff --git a/smarts/core/utils/glb.py b/smarts/core/utils/glb.py index 47e01855a4..50cd4548fa 100644 --- a/smarts/core/utils/glb.py +++ b/smarts/core/utils/glb.py @@ -19,17 +19,27 @@ # THE SOFTWARE. import math +import warnings from pathlib import Path from typing import Any, Dict, Final, List, Tuple, Union import numpy as np -import trimesh from shapely.geometry import Polygon -from trimesh.exchange import gltf from smarts.core.coordinates import BoundingBox from smarts.core.utils.geometry import triangulate_polygon +# Suppress trimesh deprecation warning +with warnings.catch_warnings(): + warnings.filterwarnings( + "ignore", + message="Please use `coo_matrix` from the `scipy.sparse` namespace, the `scipy.sparse.coo` namespace is deprecated.", + category=DeprecationWarning, + ) + import trimesh # only suppress the warnings caused by trimesh + from trimesh.exchange import gltf + from trimesh.visual.material import PBRMaterial + OLD_TRIMESH: Final[bool] = tuple(int(d) for d in trimesh.__version__.split(".")) <= ( 3, 9, @@ -135,11 +145,9 @@ def make_map_glb( scene = trimesh.Scene(metadata=metadata) meshes = _generate_meshes_from_polygons(polygons) + material = PBRMaterial("RoadDefault") for mesh in meshes: - mesh.visual = trimesh.visual.TextureVisuals( - material=trimesh.visual.material.PBRMaterial() - ) - + mesh.visual.material = material road_id = mesh.metadata["road_id"] lane_id = mesh.metadata.get("lane_id") name = str(road_id) @@ -148,13 +156,14 @@ def make_map_glb( if OLD_TRIMESH: scene.add_geometry(mesh, name, extras=mesh.metadata) else: - scene.add_geometry(mesh, name, metadata=mesh.metadata) + scene.add_geometry(mesh, name, geom_name=name, metadata=mesh.metadata) return GLBData(gltf.export_glb(scene, include_normals=True)) def make_road_line_glb(lines: List[List[Tuple[float, float]]]) -> GLBData: """Create a GLB file from a list of road/lane lines.""" scene = trimesh.Scene() + material = trimesh.visual.material.PBRMaterial() for line_pts in lines: vertices = [(*pt, 0.1) for pt in line_pts] point_cloud = trimesh.PointCloud(vertices=vertices) diff --git a/smarts/core/utils/iteration_tools.py b/smarts/core/utils/iteration_tools.py new file mode 100644 index 0000000000..d91a892a7e --- /dev/null +++ b/smarts/core/utils/iteration_tools.py @@ -0,0 +1,37 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Callable, Optional + + +def duplicates(iterable, key: Optional[Callable] = None): + """Finds all values that are duplicates.""" + seen = set() + + matcher = iterable + if key is not None: + matcher = map(key, iterable) + + for v, m in zip(iterable, matcher): + if m in seen: + yield m, v + else: + seen.add(m) diff --git a/smarts/core/utils/key_wrapper.py b/smarts/core/utils/key_wrapper.py index fb2570defb..ea5afc6462 100644 --- a/smarts/core/utils/key_wrapper.py +++ b/smarts/core/utils/key_wrapper.py @@ -38,5 +38,4 @@ def __len__(self): def insert(self, index: int, item): """Insert an item into the sequence.""" - print("asked to insert %s at index%d" % (item, index)) self.it.insert(index, item) diff --git a/smarts/core/utils/observations.py b/smarts/core/utils/observations.py new file mode 100644 index 0000000000..88bffc6bda --- /dev/null +++ b/smarts/core/utils/observations.py @@ -0,0 +1,122 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Sequence + +import numpy as np + + +def replace_rgb_image_color( + rgb: np.ndarray, + old_color: Sequence[np.ndarray], + new_color: np.ndarray, + mask: np.ndarray = np.ma.nomask, +) -> np.ndarray: + """Convert pixels of value `old_color` to `new_color` within the masked + region in the received RGB image. + + Args: + rgb (np.ndarray): RGB image. Shape = (m,n,3). + old_color (Sequence[np.ndarray]): List of old colors to be removed from the RGB image. Shape = (3,). + new_color (np.ndarray): New color to be added to the RGB image. Shape = (3,). + mask (np.ndarray, optional): Valid regions for color replacement. Shape = (m,n,3). + Defaults to np.ma.nomask . + + Returns: + np.ndarray: RGB image with `old_color` pixels changed to `new_color` + within the masked region. Shape = (m,n,3). + """ + # fmt: off + assert all(color.shape == (3,) for color in old_color), ( + f"Expected old_color to be of shape (3,), but got {[color.shape for color in old_color]}.") + assert new_color.shape == (3,), ( + f"Expected new_color to be of shape (3,), but got {new_color.shape}.") + + nc = new_color.reshape((1, 1, 3)) + nc_array = np.full_like(rgb, nc) + rgb_masked = np.ma.MaskedArray(data=rgb, mask=mask) + + rgb_condition = rgb_masked + result = rgb + for color in old_color: + result = np.ma.where((rgb_condition == color.reshape((1, 1, 3))).all(axis=-1)[..., None], nc_array, result) + + return result + # fmt: on + + +def points_to_pixels( + points: np.ndarray, + center_position: np.ndarray, + heading: float, + width: int, + height: int, + resolution: float, +) -> np.ndarray: + """Converts points into pixel coordinates in order to superimpose the + points onto the RGB image. + + Args: + points (np.ndarray): Array of points. Shape (n,3). + ego_pos (np.ndarray): Ego position. Shape = (3,). + ego_heading (float): Ego heading in radians. + w (int): Width of RGB image + h (int): Height of RGB image. + res (float): Resolution of RGB image in meters/pixels. Computed as + ground_size/image_size. + + Returns: + np.ndarray: Array of point coordinates on the RGB image. Shape = (m,3). + """ + # fmt: off + mask = np.array([not all(point == np.zeros(3,)) for point in points], dtype=bool) + points_nonzero = points[mask] + points_delta = points_nonzero - center_position + points_rotated = rotate_axes(points_delta, theta=heading) + points_pixels = points_rotated / np.array([resolution, resolution, resolution]) + points_overlay = np.array([width / 2, height / 2, 0]) + points_pixels * np.array([1, -1, 1]) + points_rfloat = np.rint(points_overlay) + points_valid = points_rfloat[(points_rfloat[:,0] >= 0) & (points_rfloat[:,0] < width) & (points_rfloat[:,1] >= 0) & (points_rfloat[:,1] < height)] + points_rint = points_valid.astype(int) + return points_rint + # fmt: on + + +def rotate_axes(points: np.ndarray, theta: float) -> np.ndarray: + """A counterclockwise rotation of the x-y axes by an angle theta θ about + the z-axis. + + Args: + points (np.ndarray): x,y,z coordinates in original axes. Shape = (n,3). + theta (np.float): Axes rotation angle in radians. + + Returns: + np.ndarray: x,y,z coordinates in rotated axes. Shape = (n,3). + """ + # fmt: off + theta = (theta + np.pi) % (2 * np.pi) - np.pi + ct, st = np.cos(theta), np.sin(theta) + R = np.array([[ ct, st, 0], + [-st, ct, 0], + [ 0, 0, 1]]) + rotated_points = (R.dot(points.T)).T + return rotated_points + # fmt: on diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index a5479cb3b5..3ee9346817 100644 --- a/smarts/core/vehicle.py +++ b/smarts/core/vehicle.py @@ -21,19 +21,21 @@ import importlib.resources as pkg_resources import logging +import warnings from functools import lru_cache, partial from typing import TYPE_CHECKING, Dict, List, Optional, Tuple, Union import numpy as np import smarts.assets.vehicles.visual_model as smarts_vehicle_visuals +from smarts.core.sensor import CustomRenderSensor, OcclusionMapSensor from . import config from .actor import ActorRole from .chassis import AckermannChassis, BoxChassis, Chassis from .colors import SceneColors from .coordinates import Dimensions, Heading, Pose -from .sensors import ( +from .sensor import ( AccelerometerSensor, DrivableAreaGridMapSensor, DrivenPathSensor, @@ -72,6 +74,7 @@ class Vehicle: "driven_path_sensor", "trip_meter_sensor", "drivable_area_grid_map_sensor", + "occlusion_map_sensor", "neighborhood_vehicle_states_sensor", "waypoints_sensor", "road_waypoints_sensor", @@ -79,6 +82,9 @@ class Vehicle: "lane_position_sensor", "via_sensor", "signals_sensor", + ) + tuple( + f"custom_render{i}_sensor" + for i in range(config()("core", "max_custom_image_sensors", cast=int)) ) def __init__( @@ -296,7 +302,7 @@ def sensor_names(self) -> Tuple[str]: @staticmethod def agent_vehicle_dims( - mission: "plan.Mission", default: Optional[str] = None + mission: "plan.NavigationMission", default: Optional[str] = None ) -> Dimensions: """Get the vehicle dimensions from the mission requirements. Args: @@ -348,7 +354,9 @@ def add_sensor_if_needed( condition: bool = True, **kwargs, ): - assert sensor_name in cls._sensor_names + assert ( + sensor_name in cls._sensor_names + ), f"{sensor_name}:{cls._sensor_names}" if ( replace or has_no_sensors @@ -391,7 +399,7 @@ def add_sensor_if_needed( ) # DrivableAreaGridMapSensor if agent_interface.drivable_area_grid_map: - if not sim.renderer: + if not sim.renderer_ref: raise RendererException.required_to("add a drivable_area_grid_map") add_sensor_if_needed( DrivableAreaGridMapSensor, @@ -401,11 +409,11 @@ def add_sensor_if_needed( width=agent_interface.drivable_area_grid_map.width, height=agent_interface.drivable_area_grid_map.height, resolution=agent_interface.drivable_area_grid_map.resolution, - renderer=sim.renderer, + renderer=sim.renderer_ref, ) # OGMSensor if agent_interface.occupancy_grid_map: - if not sim.renderer: + if not sim.renderer_ref: raise RendererException.required_to("add an OGM") add_sensor_if_needed( OGMSensor, @@ -415,11 +423,29 @@ def add_sensor_if_needed( width=agent_interface.occupancy_grid_map.width, height=agent_interface.occupancy_grid_map.height, resolution=agent_interface.occupancy_grid_map.resolution, - renderer=sim.renderer, + renderer=sim.renderer_ref, ) + if agent_interface.occlusion_map: + if not vehicle.subscribed_to("ogm_sensor"): + warnings.warn( + "Occupancy grid map sensor must be attached to use occlusion sensor.", + category=UserWarning, + ) + else: + add_sensor_if_needed( + OcclusionMapSensor, + "occlusion_map_sensor", + vehicle_state=vehicle_state, + width=agent_interface.occlusion_map.width, + height=agent_interface.occlusion_map.height, + resolution=agent_interface.occlusion_map.resolution, + renderer=sim.renderer_ref, + ogm_sensor=vehicle.sensor_property("ogm_sensor"), + add_surface_noise=agent_interface.occlusion_map.surface_noise, + ) # RGBSensor if agent_interface.top_down_rgb: - if not sim.renderer: + if not sim.renderer_ref: raise RendererException.required_to("add an RGB camera") add_sensor_if_needed( RGBSensor, @@ -429,8 +455,32 @@ def add_sensor_if_needed( width=agent_interface.top_down_rgb.width, height=agent_interface.top_down_rgb.height, resolution=agent_interface.top_down_rgb.resolution, - renderer=sim.renderer, + renderer=sim.renderer_ref, ) + if len(agent_interface.custom_renders): + if not sim.renderer_ref: + raise RendererException.required_to("add a fragment program.") + for i, program in enumerate(agent_interface.custom_renders): + add_sensor_if_needed( + CustomRenderSensor, + f"custom_render{i}_sensor", + vehicle_state=vehicle_state, + width=program.width, + height=program.height, + resolution=program.resolution, + renderer=sim.renderer_ref, + fragment_shader_path=program.fragment_shader_path, + render_dependencies=program.dependencies, + ogm_sensor=vehicle.sensor_property("ogm_sensor", None), + top_down_rgb_sensor=vehicle.sensor_property("rgb_sensor", None), + drivable_area_grid_map_sensor=vehicle.sensor_property( + "drivable_area_grid_map_sensor", default=None + ), + occlusion_map_sensor=vehicle.sensor_property( + "occlusion_map_sensor", default=None + ), + name=program.name, + ) if agent_interface.lidar_point_cloud: add_sensor_if_needed( LidarSensor, @@ -520,7 +570,7 @@ def swap_chassis(self, chassis: Chassis): self._chassis.teardown() self._chassis = chassis - def teardown(self, renderer, exclude_chassis=False): + def teardown(self, renderer: RendererBase, exclude_chassis: bool = False): """Clean up internal resources""" if not exclude_chassis: self._chassis.teardown() @@ -528,7 +578,7 @@ def teardown(self, renderer, exclude_chassis=False): renderer.remove_vehicle_node(self._id) self._initialized = False - def attach_sensor(self, sensor, sensor_name): + def attach_sensor(self, sensor: Sensor, sensor_name: str): """replace previously-attached sensor with this one (to allow updating its parameters). Sensors might have been attached to a non-agent vehicle @@ -542,7 +592,7 @@ def attach_sensor(self, sensor, sensor_name): setattr(self, f"_{sensor_name}", sensor) self._sensors[sensor_name] = sensor - def detach_sensor(self, sensor_name): + def detach_sensor(self, sensor_name: str): """Detach a sensor by name.""" self._log.debug("Removed existing %s on vehicle %s", sensor_name, self.id) sensor = getattr(self, f"_{sensor_name}", None) @@ -551,15 +601,21 @@ def detach_sensor(self, sensor_name): del self._sensors[sensor_name] return sensor - def subscribed_to(self, sensor_name): + def subscribed_to(self, sensor_name: str): """Confirm if the sensor is subscribed.""" sensor = getattr(self, f"_{sensor_name}", None) return sensor is not None - def sensor_property(self, sensor_name): + def sensor_property( + self, + sensor_name: str, + default: Optional[Union[Sensor, Ellipsis.__class__]] = ..., + ): """Call a sensor by name.""" - sensor = getattr(self, f"_{sensor_name}", None) - assert sensor is not None, f"'{sensor_name}' is not attached to '{self.id}'" + sensor = getattr(self, f"_{sensor_name}", None if default is ... else default) + assert ( + sensor is not None or default is not ... + ), f"'{sensor_name}' is not attached to '{self.id}'" return sensor def _meta_create_instance_sensor_functions(self): diff --git a/smarts/core/vehicle_index.py b/smarts/core/vehicle_index.py index 8c4f954d9f..05d3a01020 100644 --- a/smarts/core/vehicle_index.py +++ b/smarts/core/vehicle_index.py @@ -35,10 +35,12 @@ Any, Dict, FrozenSet, + Iterable, Iterator, List, NamedTuple, Optional, + Sequence, Set, Tuple, Union, @@ -74,21 +76,22 @@ VEHICLE_INDEX_ID_LENGTH = 128 -def _2id(id_: str): +def _2id(id_: str) -> bytes: separator = b"$" assert len(id_) <= VEHICLE_INDEX_ID_LENGTH - len(separator), id_ if not isinstance(id_, bytes): id_ = bytes(id_.encode()) + assert isinstance(id_, bytes) return (separator + id_).zfill(VEHICLE_INDEX_ID_LENGTH - len(separator)) class _ControlEntity(NamedTuple): - vehicle_id: Union[bytes, str] - owner_id: Union[bytes, str] + vehicle_id: bytes + owner_id: bytes role: ActorRole - shadower_id: Union[bytes, str] + shadower_id: bytes # Applies to shadower and owner # TODO: Consider moving this to an ActorRole field is_boid: bool @@ -109,13 +112,13 @@ def __init__(self): # Fixed-length ID to original ID # TODO: This quitely breaks if owner and vehicle IDs are the same. It assumes # global uniqueness. - self._2id_to_id = {} + self._2id_to_id: Dict[bytes, str] = {} # {vehicle_id (fixed-length): } - self._vehicles: Dict[str, Vehicle] = {} + self._vehicles: Dict[bytes, Vehicle] = {} # {vehicle_id (fixed-length): } - self._controller_states = {} + self._controller_states: Dict[bytes, ControllerState] = {} # Loaded from yaml file on scenario reset self._vehicle_definitions: resources.VehicleDefinitions = ( @@ -127,7 +130,7 @@ def identity(cls): """Returns an empty identity index.""" return cls() - def __sub__(self, other: "VehicleIndex") -> "VehicleIndex": + def __sub__(self, other: VehicleIndex) -> VehicleIndex: vehicle_ids = set(self._controlled_by["vehicle_id"]) - set( other._controlled_by["vehicle_id"] ) @@ -135,7 +138,7 @@ def __sub__(self, other: "VehicleIndex") -> "VehicleIndex": vehicle_ids = [self._2id_to_id[id_] for id_ in vehicle_ids] return self._subset(vehicle_ids) - def __and__(self, other: "VehicleIndex") -> "VehicleIndex": + def __and__(self, other: VehicleIndex) -> VehicleIndex: vehicle_ids = set(self._controlled_by["vehicle_id"]) & set( other._controlled_by["vehicle_id"] ) @@ -143,28 +146,28 @@ def __and__(self, other: "VehicleIndex") -> "VehicleIndex": vehicle_ids = [self._2id_to_id[id_] for id_ in vehicle_ids] return self._subset(vehicle_ids) - def _subset(self, vehicle_ids): + def _subset(self, vehicle_ids: Iterable[str]): assert self.vehicle_ids().issuperset( vehicle_ids ), f"{', '.join(list(self.vehicle_ids())[:3])} ⊅ {', '.join(list(vehicle_ids)[:3])}" - vehicle_ids = [_2id(id_) for id_ in vehicle_ids] + b_vehicle_ids = [_2id(id_) for id_ in vehicle_ids] index = VehicleIndex() indices = np.isin( - self._controlled_by["vehicle_id"], vehicle_ids, assume_unique=True + self._controlled_by["vehicle_id"], b_vehicle_ids, assume_unique=True ) index._controlled_by = self._controlled_by[indices] - index._2id_to_id = {id_: self._2id_to_id[id_] for id_ in vehicle_ids} - index._vehicles = {id_: self._vehicles[id_] for id_ in vehicle_ids} + index._2id_to_id = {id_: self._2id_to_id[id_] for id_ in b_vehicle_ids} + index._vehicles = {id_: self._vehicles[id_] for id_ in b_vehicle_ids} index._controller_states = { id_: self._controller_states[id_] - for id_ in vehicle_ids + for id_ in b_vehicle_ids if id_ in self._controller_states } return index - def __deepcopy__(self, memo): + def __deepcopy__(self, memo) -> VehicleIndex: result = self.__class__.__new__(self.__class__) memo[id(self)] = result @@ -210,11 +213,11 @@ def social_vehicle_ids( } @cache - def vehicle_is_hijacked_or_shadowed(self, vehicle_id) -> Tuple[bool, bool]: + def vehicle_is_hijacked_or_shadowed(self, vehicle_id: str) -> Tuple[bool, bool]: """Determine if a vehicle is either taken over by an owner or watched.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) - v_index = self._controlled_by["vehicle_id"] == vehicle_id + v_index = self._controlled_by["vehicle_id"] == b_vehicle_id if not np.any(v_index): return False, False @@ -225,26 +228,28 @@ def vehicle_is_hijacked_or_shadowed(self, vehicle_id) -> Tuple[bool, bool]: return bool(vehicle["is_hijacked"]), bool(vehicle["shadower_id"]) @cache - def vehicle_ids_by_owner_id(self, owner_id, include_shadowers=False) -> List[str]: + def vehicle_ids_by_owner_id( + self, owner_id: str, include_shadowers: bool = False + ) -> List[str]: """Returns all vehicles for the given owner ID as a list. This is most applicable when an agent is controlling multiple vehicles (e.g. with boids). """ - owner_id = _2id(owner_id) + b_owner_id = _2id(owner_id) - v_index = self._controlled_by["owner_id"] == owner_id + v_index = self._controlled_by["owner_id"] == b_owner_id if include_shadowers: - v_index = v_index | (self._controlled_by["shadower_id"] == owner_id) + v_index = v_index | (self._controlled_by["shadower_id"] == b_owner_id) - vehicle_ids = self._controlled_by[v_index]["vehicle_id"] - return [self._2id_to_id[id_] for id_ in vehicle_ids] + b_vehicle_ids = self._controlled_by[v_index]["vehicle_id"] + return [self._2id_to_id[id_] for id_ in b_vehicle_ids] @cache - def owner_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: + def owner_id_from_vehicle_id(self, vehicle_id: str) -> Optional[str]: """Find the owner id associated with the given vehicle.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) owner_ids = self._controlled_by[ - self._controlled_by["vehicle_id"] == vehicle_id + self._controlled_by["vehicle_id"] == b_vehicle_id ]["owner_id"] if owner_ids: @@ -253,12 +258,12 @@ def owner_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: return None @cache - def shadower_id_from_vehicle_id(self, vehicle_id) -> Optional[str]: + def shadower_id_from_vehicle_id(self, vehicle_id: str) -> Optional[str]: """Find the first shadowing entity watching a vehicle.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) shadower_ids = self._controlled_by[ - self._controlled_by["vehicle_id"] == vehicle_id + self._controlled_by["vehicle_id"] == b_vehicle_id ]["shadower_id"] if shadower_ids: @@ -276,17 +281,17 @@ def shadower_ids(self) -> Set[str]: ) @cache - def vehicle_position(self, vehicle_id): + def vehicle_position(self, vehicle_id: str): """Find the position of the given vehicle.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) positions = self._controlled_by[ - self._controlled_by["vehicle_id"] == vehicle_id + self._controlled_by["vehicle_id"] == b_vehicle_id ]["position"] return positions[0] if len(positions) > 0 else None - def vehicles_by_owner_id(self, owner_id, include_shadowers=False): + def vehicles_by_owner_id(self, owner_id: str, include_shadowers: bool = False): """Find vehicles associated with the given owner id. Args: owner_id: @@ -310,7 +315,7 @@ def vehicle_is_shadowed(self, vehicle_id: str) -> bool: return is_shadowed @property - def vehicles(self): + def vehicles(self) -> List[Vehicle]: """A list of all existing vehicles.""" return list(self._vehicles.values()) @@ -320,45 +325,54 @@ def vehicleitems(self) -> Iterator[Tuple[str, Vehicle]]: return map(lambda x: (self._2id_to_id[x[0]], x[1]), self._vehicles.items()) @cache - def vehicle_by_id(self, vehicle_id, default=...): + def vehicle_by_id( + self, + vehicle_id: str, + default: Optional[Union[Vehicle, Ellipsis.__class__]] = ..., + ): """Get a vehicle by its id.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) if default is ...: - return self._vehicles[vehicle_id] - return self._vehicles.get(vehicle_id, default) + return self._vehicles[b_vehicle_id] + return self._vehicles.get(b_vehicle_id, default) @clear_cache - def teardown_vehicles_by_vehicle_ids(self, vehicle_ids, renderer: Optional[object]): + def teardown_vehicles_by_vehicle_ids( + self, vehicle_ids: Sequence[str], renderer: Optional[RendererBase] + ): """Terminate and remove a vehicle from the index using its id.""" self._log.debug("Tearing down vehicle ids: %s", vehicle_ids) - vehicle_ids = [_2id(id_) for id_ in vehicle_ids] - if len(vehicle_ids) == 0: + b_vehicle_ids = [_2id(id_) for id_ in vehicle_ids] + if len(b_vehicle_ids) == 0: return - for vehicle_id in vehicle_ids: - vehicle = self._vehicles.pop(vehicle_id, None) + for b_vehicle_id in b_vehicle_ids: + vehicle = self._vehicles.pop(b_vehicle_id, None) if vehicle is not None: vehicle.teardown(renderer=renderer) # popping since sensor_states/controller_states may not include the # vehicle if it's not being controlled by an agent - self._controller_states.pop(vehicle_id, None) + self._controller_states.pop(b_vehicle_id, None) # TODO: This stores agents as well; those aren't being cleaned-up - self._2id_to_id.pop(vehicle_id, None) + self._2id_to_id.pop(b_vehicle_id, None) remove_vehicle_indices = np.isin( - self._controlled_by["vehicle_id"], vehicle_ids, assume_unique=True + self._controlled_by["vehicle_id"], b_vehicle_ids, assume_unique=True ) self._controlled_by = self._controlled_by[~remove_vehicle_indices] def teardown_vehicles_by_owner_ids( - self, owner_ids, renderer, include_shadowing=True - ): + self, + owner_ids: Iterable[str], + renderer: RendererBase, + include_shadowing: bool = True, + ) -> List[str]: """Terminate and remove all vehicles associated with an owner id.""" - vehicle_ids = [] + vehicle_ids: List[str] = [] for owner_id in owner_ids: vehicle_ids.extend( [v.id for v in self.vehicles_by_owner_id(owner_id, include_shadowing)] @@ -379,7 +393,7 @@ def sync(self): ) @clear_cache - def teardown(self, renderer): + def teardown(self, renderer: RendererBase): """Clean up resources, resetting the index.""" self._controlled_by = VehicleIndex._build_empty_controlled_by() @@ -394,19 +408,18 @@ def teardown(self, renderer): def start_agent_observation( self, sim: SMARTS, - vehicle_id, - agent_id, + vehicle_id: str, + agent_id: str, agent_interface: AgentInterface, plan: "plan.Plan", - boid=False, - initialize_sensors=True, + boid: bool = False, + initialize_sensors: bool = True, ): """Associate an agent to a vehicle. Set up any needed sensor requirements.""" - original_agent_id = agent_id - vehicle_id, agent_id = _2id(vehicle_id), _2id(agent_id) - self._2id_to_id[agent_id] = original_agent_id + b_vehicle_id, b_agent_id = _2id(vehicle_id), _2id(agent_id) + self._2id_to_id[b_agent_id] = agent_id - vehicle = self._vehicles[vehicle_id] + vehicle = self._vehicles[b_vehicle_id] sim.sensor_manager.add_sensor_state( vehicle.id, @@ -420,14 +433,14 @@ def start_agent_observation( sim.sensor_manager, sim, vehicle, agent_interface ) - self._controller_states[vehicle_id] = ControllerState.from_action_space( + self._controller_states[b_vehicle_id] = ControllerState.from_action_space( agent_interface.action, vehicle.pose, sim ) - v_index = self._controlled_by["vehicle_id"] == vehicle_id + v_index = self._controlled_by["vehicle_id"] == b_vehicle_id entity = _ControlEntity(*self._controlled_by[v_index][0]) self._controlled_by[v_index] = tuple( - entity._replace(shadower_id=agent_id, is_boid=boid) + entity._replace(shadower_id=b_agent_id, is_boid=boid) ) # XXX: We are not giving the vehicle a chassis here but rather later @@ -441,11 +454,11 @@ def start_agent_observation( def switch_control_to_agent( self, sim: SMARTS, - vehicle_id, - agent_id, - boid=False, - hijacking=False, - recreate=False, + vehicle_id: str, + agent_id: str, + boid: bool = False, + hijacking: bool = False, + recreate: bool = False, agent_interface: Optional[AgentInterface] = None, ): """Give control of the specified vehicle to the specified agent. @@ -467,16 +480,16 @@ def switch_control_to_agent( """ self._log.debug("Switching control of '%s' to '%s'", vehicle_id, agent_id) - vehicle_id, agent_id = _2id(vehicle_id), _2id(agent_id) + b_vehicle_id, b_agent_id = _2id(vehicle_id), _2id(agent_id) if recreate: # XXX: Recreate is presently broken for bubbles because it impacts the # sumo traffic sim sync(...) logic in how it detects a vehicle as # being hijacked vs joining. Presently it's still used for trapping. return self._switch_control_to_agent_recreate( - sim, vehicle_id, agent_id, boid, hijacking + sim, b_vehicle_id, b_agent_id, boid, hijacking ) - vehicle = self._vehicles[vehicle_id] + vehicle = self._vehicles[b_vehicle_id] chassis = None if agent_interface and agent_interface.action in sim.dynamic_action_spaces: vehicle_definition = self._vehicle_definitions.load_vehicle_definition( @@ -505,13 +518,13 @@ def switch_control_to_agent( ) vehicle.swap_chassis(chassis) - v_index = self._controlled_by["vehicle_id"] == vehicle_id + v_index = self._controlled_by["vehicle_id"] == b_vehicle_id entity = _ControlEntity(*self._controlled_by[v_index][0]) role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent self._controlled_by[v_index] = tuple( entity._replace( role=role, - owner_id=agent_id, + owner_id=b_agent_id, shadower_id=b"", is_boid=boid, is_hijacked=hijacking, @@ -533,25 +546,25 @@ def stop_shadowing(self, shadower_id: str, vehicle_id: Optional[str] = None): v_index = self._controlled_by["shadower_id"] == shadower_id if vehicle_id: - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) # This multiplication finds overlap of "shadower_id" and "vehicle_id" - v_index = (self._controlled_by["vehicle_id"] == vehicle_id) * v_index + v_index = (self._controlled_by["vehicle_id"] == b_vehicle_id) * v_index for entity in self._controlled_by[v_index]: entity = _ControlEntity(*entity) self._controlled_by[v_index] = tuple(entity._replace(shadower_id=b"")) @clear_cache - def stop_agent_observation(self, vehicle_id) -> Vehicle: + def stop_agent_observation(self, vehicle_id: str) -> Vehicle: """Strip all sensors from a vehicle and stop all owners from watching the vehicle.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) - vehicle = self._vehicles[vehicle_id] + vehicle = self._vehicles[b_vehicle_id] - v_index = self._controlled_by["vehicle_id"] == vehicle_id + v_index = self._controlled_by["vehicle_id"] == b_vehicle_id entity = self._controlled_by[v_index][0] entity = _ControlEntity(*entity) - self._controlled_by[v_index] = tuple(entity._replace(shadower_id="")) + self._controlled_by[v_index] = tuple(entity._replace(shadower_id=b"")) return vehicle @@ -602,14 +615,14 @@ def relinquish_agent_control( def attach_sensors_to_vehicle( self, sim: SMARTS, - vehicle_id, + vehicle_id: str, agent_interface: AgentInterface, plan: "plan.Plan", ): """Attach sensors as per the agent interface requirements to the specified vehicle.""" - vehicle_id = _2id(vehicle_id) + b_vehicle_id = _2id(vehicle_id) - vehicle = self._vehicles[vehicle_id] + vehicle = self._vehicles[b_vehicle_id] Vehicle.attach_sensors_to_vehicle( sim.sensor_manager, sim, @@ -623,16 +636,21 @@ def attach_sensors_to_vehicle( plan_frame=plan.frame(), ), ) - self._controller_states[vehicle_id] = ControllerState.from_action_space( + self._controller_states[b_vehicle_id] = ControllerState.from_action_space( agent_interface.action, vehicle.pose, sim ) def _switch_control_to_agent_recreate( - self, sim: SMARTS, vehicle_id, agent_id, boid: bool, hijacking: bool + self, + sim: SMARTS, + b_vehicle_id: bytes, + b_agent_id: bytes, + boid: bool, + hijacking: bool, ): # XXX: vehicle_id and agent_id are already fixed-length as this is an internal # method. - agent_id = self._2id_to_id[agent_id] + agent_id = self._2id_to_id[b_agent_id] # TODO: There existed a SUMO connection error bug # (https://gitlab.smartsai.xyz/smarts/SMARTS/-/issues/671) that occurred @@ -645,10 +663,10 @@ def _switch_control_to_agent_recreate( assert ( agent_interface is not None ), f"Missing agent_interface for agent_id={agent_id}" - vehicle = self._vehicles[vehicle_id] + vehicle = self._vehicles[b_vehicle_id] sensor_state = sim.sensor_manager.sensor_state_for_actor_id(vehicle.id) assert sensor_state is not None - controller_state = self._controller_states[vehicle_id] + controller_state = self._controller_states[b_vehicle_id] plan = sensor_state.get_plan(sim.road_map) vehicle_definition = self._vehicle_definitions.load_vehicle_definition( @@ -713,12 +731,12 @@ def _build_agent_vehicle( action: Optional[ActionSpaceType], vehicle_type: str, vehicle_class: str, - mission: plan.Mission, + mission: plan.NavigationMission, vehicle_dynamics_filepath: Optional[str], tire_filepath: str, visual_model_filepath: str, trainable: bool, - surface_patches: List[Dict[str, Any]], + surface_patches: Sequence[Dict[str, Any]], initial_speed: Optional[float] = None, ) -> Vehicle: """Create a new vehicle and set up sensors and planning information as required by the @@ -787,15 +805,15 @@ def _build_agent_vehicle( def build_agent_vehicle( self, sim: SMARTS, - agent_id, + agent_id: str, agent_interface: AgentInterface, plan: "plan.Plan", trainable: bool, initial_speed: Optional[float] = None, boid: bool = False, *, - vehicle_id=None, - ): + vehicle_id: Optional[str] = None, + ) -> Vehicle: """Build an entirely new vehicle for an agent.""" vehicle_definition = self._vehicle_definitions.load_vehicle_definition( agent_interface.vehicle_class @@ -838,7 +856,7 @@ def build_agent_vehicle( def _enfranchise_agent( self, sim: SMARTS, - agent_id, + agent_id: str, agent_interface: AgentInterface, vehicle: Vehicle, controller_state: ControllerState, @@ -847,7 +865,6 @@ def _enfranchise_agent( hijacking: bool = False, ): # XXX: agent_id must be the original agent_id (not the fixed _2id(...)) - original_agent_id = agent_id Vehicle.attach_sensors_to_vehicle( sim.sensor_manager, sim, vehicle, agent_interface @@ -856,19 +873,19 @@ def _enfranchise_agent( vehicle.create_renderer_node(sim.renderer_ref) sim.renderer.begin_rendering_vehicle(vehicle.id, is_agent=True) - vehicle_id = _2id(vehicle.id) - agent_id = _2id(original_agent_id) + b_vehicle_id = _2id(vehicle.id) + b_agent_id = _2id(agent_id) sim.sensor_manager.add_sensor_state(vehicle.id, sensor_state) - self._controller_states[vehicle_id] = controller_state - self._vehicles[vehicle_id] = vehicle - self._2id_to_id[vehicle_id] = vehicle.id - self._2id_to_id[agent_id] = original_agent_id + self._controller_states[b_vehicle_id] = controller_state + self._vehicles[b_vehicle_id] = vehicle + self._2id_to_id[b_vehicle_id] = vehicle.id + self._2id_to_id[b_agent_id] = agent_id role = ActorRole.SocialAgent if hijacking else ActorRole.EgoAgent entity = _ControlEntity( - vehicle_id=vehicle_id, - owner_id=agent_id, + vehicle_id=b_vehicle_id, + owner_id=b_agent_id, role=role, shadower_id=b"", is_boid=boid, @@ -901,7 +918,11 @@ def _build_social_vehicle( @clear_cache def build_social_vehicle( - self, sim: SMARTS, vehicle_state: VehicleState, owner_id, vehicle_id=None + self, + sim: SMARTS, + vehicle_state: VehicleState, + owner_id: str, + vehicle_id: Optional[str] = None, ) -> Vehicle: """Build an entirely new vehicle for a social agent.""" if vehicle_id is None: @@ -913,13 +934,13 @@ def build_social_vehicle( vehicle_state, ) - vehicle_id, owner_id = _2id(vehicle_id), _2id(owner_id) if owner_id else b"" + b_vehicle_id, b_owner_id = _2id(vehicle_id), _2id(owner_id) if owner_id else b"" if sim.is_rendering: vehicle.create_renderer_node(sim.renderer_ref) sim.renderer.begin_rendering_vehicle(vehicle.id, is_agent=False) - self._vehicles[vehicle_id] = vehicle - self._2id_to_id[vehicle_id] = vehicle.id + self._vehicles[b_vehicle_id] = vehicle + self._2id_to_id[b_vehicle_id] = vehicle.id role = vehicle_state.role assert role not in ( @@ -927,8 +948,8 @@ def build_social_vehicle( ActorRole.SocialAgent, ), f"role={role} from {vehicle_state.source}" entity = _ControlEntity( - vehicle_id=vehicle_id, - owner_id=owner_id, + vehicle_id=b_vehicle_id, + owner_id=b_owner_id, role=role, shadower_id=b"", is_boid=False, @@ -950,8 +971,7 @@ def begin_rendering_vehicles(self, renderer: RendererBase): @cache def controller_state_for_vehicle_id(self, vehicle_id: str) -> ControllerState: """Retrieve the controller state of the given vehicle.""" - vehicle_id = _2id(vehicle_id) - return self._controller_states[vehicle_id] + return self._controller_states[_2id(vehicle_id)] def load_vehicle_definitions_list(self, vehicle_definitions_filepath: str): """Loads in a list of vehicle definitions.""" @@ -977,7 +997,7 @@ def _build_empty_controlled_by(): ], ) - def __repr__(self): + def __repr__(self) -> str: io = StringIO("") n_columns = len(self._controlled_by.dtype.names) diff --git a/smarts/core/vehicle_state.py b/smarts/core/vehicle_state.py index dca4b935bd..c1a00545c6 100644 --- a/smarts/core/vehicle_state.py +++ b/smarts/core/vehicle_state.py @@ -27,6 +27,8 @@ from shapely.geometry import Polygon from shapely.geometry import box as shapely_box +from smarts.core.coordinates import Dimensions + from .actor import ActorState from .colors import SceneColors from .coordinates import Dimensions, Heading, Pose @@ -128,6 +130,28 @@ def __eq__(self, __o: object): and self.pose == __o.pose ) + def get_pose(self) -> Optional[Pose]: + return self.pose + + def get_dimensions(self) -> Optional[Dimensions]: + return self.dimensions + + def linear_velocity_tuple(self) -> Optional[Tuple[float, float, float]]: + """Generates a tuple representation of linear velocity with standard python types.""" + return ( + None + if self.linear_velocity is None + else tuple(float(f) for f in self.linear_velocity) + ) + + def angular_velocity_tuple(self) -> Optional[Tuple[float, float, float]]: + """Generates a tuple representation of angular velocity with standard python types.""" + return ( + None + if self.angular_velocity is None + else tuple(float(f) for f in self.angular_velocity) + ) + @property def bounding_box_points( self, diff --git a/smarts/engine.ini b/smarts/engine.ini index 1cb624a807..031fe31fba 100644 --- a/smarts/engine.ini +++ b/smarts/engine.ini @@ -6,12 +6,10 @@ default_agent_vehicle = passenger debug = False observation_workers = 0 reset_retries = 0 +sensor_parallelization = mp [controllers] [physics] -max_pybullet_freq = 240 [providers] [rendering] [ray] -log_to_driver=False -[traffic] -traci_retries=5 \ No newline at end of file +log_to_driver=False \ No newline at end of file diff --git a/smarts/env/gymnasium/wrappers/metric/costs.py b/smarts/env/gymnasium/wrappers/metric/costs.py index 1048cb75ae..6cb06580f9 100644 --- a/smarts/env/gymnasium/wrappers/metric/costs.py +++ b/smarts/env/gymnasium/wrappers/metric/costs.py @@ -27,7 +27,7 @@ from smarts.core.coordinates import Heading, Point, RefLinePoint from smarts.core.observations import Observation -from smarts.core.plan import Mission, Plan, PositionalGoal, Start +from smarts.core.plan import NavigationMission, Plan, PositionalGoal, Start from smarts.core.road_map import RoadMap from smarts.core.utils.core_math import running_mean from smarts.core.vehicle_index import VehicleIndex @@ -74,14 +74,14 @@ def func( jerk = 0 acc = 0 if len(vehicle_pos) >= 3: - disp_0 = np.linalg.norm(vehicle_pos[0] - vehicle_pos[1]) - disp_1 = np.linalg.norm(vehicle_pos[1] - vehicle_pos[2]) + disp_0 = np.linalg.norm(np.subtract(vehicle_pos[0], vehicle_pos[1])) + disp_1 = np.linalg.norm(np.subtract(vehicle_pos[1], vehicle_pos[2])) speed_0 = disp_0 / dt speed_1 = disp_1 / dt if valid_0 := (disp_0 > min_disp and disp_1 > min_disp): acc = (speed_0 - speed_1) / dt if valid_0 and len(vehicle_pos) == 4: - disp_2 = np.linalg.norm(vehicle_pos[2] - vehicle_pos[3]) + disp_2 = np.linalg.norm(np.subtract(vehicle_pos[2], vehicle_pos[3])) speed_2 = disp_2 / dt acc_1 = (speed_1 - speed_2) / dt if disp_2 > min_disp: @@ -238,7 +238,8 @@ def func( # Filter neighbors by distance. nghbs_state = [ - (nghb, np.linalg.norm(nghb.position - ego_pos)) for nghb in nghbs + (nghb, np.linalg.norm(np.subtract(nghb.position, ego_pos))) + for nghb in nghbs ] nghbs_state = [ nghb_state @@ -261,7 +262,7 @@ def func( # Neighbors's angle with respect to the ego's position. # Note: In np.angle(), angle is zero at positive x axis, and increases anti-clockwise. # Hence, map_angle = np.angle() - π/2 - rel_pos = np.array(nghb_state[0].position) - ego_pos + rel_pos = np.subtract(nghb_state[0].position, ego_pos) obstacle_angle = np.angle(rel_pos[0] + 1j * rel_pos[1]) - np.pi / 2 obstacle_angle = (obstacle_angle + np.pi) % (2 * np.pi) - np.pi # Relative angle is the angle rotation required by ego agent to face the obstacle. @@ -626,7 +627,7 @@ def get_dist( RoadMap.Route: Planned route between point_a and point_b. """ - mission = Mission( + mission = NavigationMission( start=Start( position=point_a.as_np_array, heading=Heading(0), diff --git a/smarts/env/tests/test_metrics.py b/smarts/env/tests/test_metrics.py index a9d4394367..99b7d63b5e 100644 --- a/smarts/env/tests/test_metrics.py +++ b/smarts/env/tests/test_metrics.py @@ -30,7 +30,7 @@ from smarts.core.agent_interface import AgentInterface, DoneCriteria from smarts.core.controllers import ActionSpaceType from smarts.core.coordinates import Heading, Point -from smarts.core.plan import EndlessGoal, Goal, Mission, PositionalGoal, Start +from smarts.core.plan import EndlessGoal, Goal, NavigationMission, PositionalGoal, Start from smarts.env.gymnasium.wrappers.metric.metrics import Metrics, MetricsError from smarts.zoo.agent_spec import AgentSpec @@ -146,7 +146,7 @@ def test_init(make_env): def _mock_mission(start: Start, goal: Goal): def func(scenario_root, agents_to_be_briefed): - return [Mission(start=start, goal=goal)] + return [NavigationMission(start=start, goal=goal)] return func diff --git a/smarts/env/utils/observation_conversion.py b/smarts/env/utils/observation_conversion.py index 0e5723342f..9de20ff3cd 100644 --- a/smarts/env/utils/observation_conversion.py +++ b/smarts/env/utils/observation_conversion.py @@ -30,7 +30,7 @@ from smarts.core.agent_interface import AgentInterface from smarts.core.observations import Observation, SignalObservation, VehicleObservation -from smarts.core.plan import Mission +from smarts.core.plan import NavigationMission from smarts.core.road_map import Waypoint _LIDAR_SHP = 300 @@ -80,7 +80,7 @@ def _format_id(lane_id: str, max_length, type_): return lane_name[:max_length] -def _format_mission(mission: Mission): +def _format_mission(mission: NavigationMission): if hasattr(mission.goal, "position"): goal_pos = np.array(mission.goal.position, dtype=np.float64) else: @@ -183,7 +183,7 @@ def _format_signals(signals: List[SignalObservation]): def _format_neighborhood_vehicle_states( - neighborhood_vehicle_states: List[VehicleObservation], + neighborhood_vehicle_states: Tuple[VehicleObservation], ): des_shp = _NEIGHBOR_SHP rcv_shp = len(neighborhood_vehicle_states) @@ -464,7 +464,7 @@ def __call__(self, agent_interface: AgentInterface) -> BaseSpaceFormat: ego_box_space_format = StandardSpaceFormat( - lambda obs: np.array(obs.ego_vehicle_state.bounding_box.as_lwh).astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.bounding_box.as_lwh, dtype=np.float32), lambda _: True, "box", _VEC3_UNSIGNED_FLOAT32_SPACE, @@ -485,21 +485,21 @@ def __call__(self, agent_interface: AgentInterface) -> BaseSpaceFormat: ) ego_linear_velocity_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.linear_velocity.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.linear_velocity, dtype=np.float32), lambda _: True, "linear_velocity", _VEC3_SIGNED_FLOAT32_SPACE, ) ego_angular_velocity_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.angular_velocity.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.angular_velocity, dtype=np.float32), lambda _: True, "angular_velocity", _VEC3_SIGNED_FLOAT32_SPACE, ) ego_position_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.position.astype(np.float64), + lambda obs: np.array(obs.ego_vehicle_state.position, dtype=np.float64), lambda _: True, "position", _VEC3_SIGNED_FLOAT64_SPACE, @@ -536,28 +536,28 @@ def __call__(self, agent_interface: AgentInterface) -> BaseSpaceFormat: ) ego_angular_acceleration_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.angular_acceleration.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.angular_acceleration, dtype=np.float32), lambda agent_interface: bool(agent_interface.accelerometer), "angular_acceleration", _VEC3_SIGNED_FLOAT32_SPACE, ) ego_angular_jerk_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.angular_jerk.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.angular_jerk, dtype=np.float32), lambda agent_interface: bool(agent_interface.accelerometer), "angular_jerk", _VEC3_SIGNED_FLOAT32_SPACE, ) ego_linear_acceleration_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.linear_acceleration.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.linear_acceleration, dtype=np.float32), lambda agent_interface: bool(agent_interface.accelerometer), "linear_acceleration", _VEC3_SIGNED_FLOAT32_SPACE, ) ego_linear_jerk_space_format = StandardSpaceFormat( - lambda obs: obs.ego_vehicle_state.linear_jerk.astype(np.float32), + lambda obs: np.array(obs.ego_vehicle_state.linear_jerk, dtype=np.float32), lambda agent_interface: bool(agent_interface.accelerometer), "linear_jerk", _VEC3_SIGNED_FLOAT32_SPACE, diff --git a/smarts/p3d/renderer.py b/smarts/p3d/renderer.py index 7eb0639897..02ddfc1e46 100644 --- a/smarts/p3d/renderer.py +++ b/smarts/p3d/renderer.py @@ -23,15 +23,17 @@ from __future__ import annotations import importlib.resources as pkg_resources +import itertools import logging import math import os import re import warnings from dataclasses import dataclass +from functools import lru_cache from pathlib import Path from threading import Lock -from typing import Literal, Optional, Tuple, Union +from typing import TYPE_CHECKING, Collection, Dict, Literal, Optional, Tuple, Union import gltf import numpy as np @@ -39,6 +41,8 @@ # pytype: disable=import-error from panda3d.core import ( + Camera, + CardMaker, FrameBufferProperties, Geom, GeomLinestrips, @@ -53,21 +57,39 @@ NodePath, OrthographicLens, Shader, + ShaderInput, Texture, WindowProperties, loadPrcFileData, ) -import smarts.assets from smarts.core import glsl from smarts.core.colors import Colors, SceneColors from smarts.core.coordinates import Point, Pose from smarts.core.masks import RenderMasks -from smarts.core.renderer_base import DEBUG_MODE, OffscreenCamera, RendererBase -from smarts.core.scenario import Scenario +from smarts.core.renderer_base import ( + DEBUG_MODE, + OffscreenCamera, + RendererBase, + RendererNotSetUpWarning, + ShaderStep, + ShaderStepBufferDependency, + ShaderStepCameraDependency, + ShaderStepDependencyBase, + ShaderStepVariableDependency, +) +from smarts.core.scenario import Scenario as StaticScenario +from smarts.core.shader_buffer import BufferID from smarts.core.signals import SignalState, signal_state_to_color +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.utils.core_logging import suppress_output from smarts.core.vehicle_state import VehicleState +if TYPE_CHECKING: + from smarts.core.agent_interface import AgentInterface + from smarts.core.observations import Observation + + # pytype: enable=import-error BACKEND_LITERALS = Literal[ @@ -97,6 +119,7 @@ def __new__(cls): ) loadPrcFileData("", "aux-display pandagl") loadPrcFileData("", "aux-display pandadx9") + loadPrcFileData("", "aux-display pandadx8") loadPrcFileData("", "aux-display pandagles") loadPrcFileData("", "aux-display pandagles2") loadPrcFileData("", "aux-display p3headlessgl") @@ -110,6 +133,9 @@ def __new__(cls): loadPrcFileData("", "audio-library-name null") loadPrcFileData("", "gl-version 3 3") loadPrcFileData("", f"notify-level {cls._debug_mode.name.lower()}") + loadPrcFileData( + "", f"default-directnotify-level {cls._debug_mode.name.lower()}" + ) loadPrcFileData("", "print-pipe-types false") # loadPrcFileData("", "basic-shaders-only #t") # https://www.panda3d.org/manual/?title=Multithreaded_Render_Pipeline @@ -146,6 +172,9 @@ def set_rendering_verbosity(cls, debug_mode: DEBUG_MODE): """Set rendering debug information verbosity.""" cls._debug_mode = debug_mode loadPrcFileData("", f"notify-level {cls._debug_mode.name.lower()}") + loadPrcFileData( + "", f"default-directnotify-level {cls._debug_mode.name.lower()}" + ) @classmethod def set_rendering_backend( @@ -156,7 +185,8 @@ def set_rendering_backend( if "__it__" not in cls.__dict__: cls._rendering_backend = rendering_backend else: - warnings.warn("Cannot apply rendering backend after setup.") + if cls._rendering_backend != rendering_backend: + warnings.warn("Cannot apply rendering backend after setup.") def destroy(self): """Destroy this renderer and clean up all remaining resources.""" @@ -193,18 +223,17 @@ def render_node(self, sim_root: NodePath): # when we call poll() here. hidden = [] with self._render_lock: - for np in self.render.children: - if np != sim_root and not np.isHidden(): - np.hide() - hidden.append(np) + for node_path in self.render.children: + if node_path != sim_root and not node_path.isHidden(): + node_path.hide() + hidden.append(node_path) self.taskMgr.mgr.poll() - for np in hidden: - np.show() + for node_path in hidden: + node_path.show() @dataclass -class P3dOffscreenCamera(OffscreenCamera): - """A camera used for rendering images to a graphics buffer.""" +class _P3DCameraMixin: camera_np: NodePath buffer: GraphicsOutput @@ -223,7 +252,7 @@ def wait_for_ram_image(self, img_format: str, retries=100): for i in range(retries): if self.tex.mightHaveRamImage(): break - self.renderer.log.debug( + getattr(self, "renderer").log.debug( f"No image available (attempt {i}/{retries}), forcing a render" ) region = self.buffer.getDisplayRegion(0) @@ -234,29 +263,24 @@ def wait_for_ram_image(self, img_format: str, retries=100): assert ram_image is not None return ram_image - def update(self, pose: Pose, height: float): - """Update the location of the camera. - Args: - pose: - The pose of the camera target. - height: - The height of the camera above the camera target. - """ - pos, heading = pose.as_panda3d() - self.camera_np.setPos(pos[0], pos[1], height) - self.camera_np.lookAt(*pos) - self.camera_np.setH(heading) - @property def image_dimensions(self): + """The dimensions of the output camera image.""" return (self.tex.getXSize(), self.tex.getYSize()) @property def position(self) -> Tuple[float, float, float]: - return self.camera_np.getPos() + """The position of the camera.""" + raise NotImplementedError() + + @property + def padding(self) -> Tuple[int, int, int, int]: + """The padding on the image. This follows the "css" convention: (top, left, bottom, right).""" + return self.tex.getPadYSize(), self.tex.getPadXSize(), 0, 0 @property def heading(self) -> float: + """The heading of this camera.""" return np.radians(self.camera_np.getH()) def teardown(self): @@ -265,7 +289,417 @@ def teardown(self): region = self.buffer.getDisplayRegion(0) region.window.clearRenderTextures() self.buffer.removeAllDisplayRegions() - self.renderer.remove_buffer(self.buffer) + getattr(self, "renderer").remove_buffer(self.buffer) + + +@dataclass +class P3DOffscreenCamera(_P3DCameraMixin, OffscreenCamera): + """A camera used for rendering images to a graphics buffer.""" + + def update(self, pose: Pose, height: float, *args, **kwargs): + """Update the location of the camera. + Args: + pose: + The pose of the camera target. + height: + The height of the camera above the camera target. + """ + pos, heading = pose.as_panda3d() + self.camera_np.setPos(pos[0], pos[1], height) + self.camera_np.lookAt(*pos) + self.camera_np.setH(heading) + + @property + def position(self) -> Tuple[float, float, float]: + return self.camera_np.getPos() + + +class _BufferAccessor: + _static_methods = {} + _acceleration_set = { + BufferID.EGO_VEHICLE_STATE_LINEAR_ACCELERATION, + BufferID.EGO_VEHICLE_STATE_ANGULAR_ACCELERATION, + } + _jerk_set = { + BufferID.EGO_VEHICLE_STATE_LINEAR_JERK, + BufferID.EGO_VEHICLE_STATE_ANGULAR_JERK, + } + _waypoints_set = { + BufferID.WAYPOINT_PATHS_POSITION, + BufferID.WAYPOINT_PATHS_HEADING, + BufferID.WAYPOINT_PATHS_LANE_ID, + BufferID.WAYPOINT_PATHS_LANE_INDEX, + BufferID.WAYPOINT_PATHS_LANE_OFFSET, + BufferID.WAYPOINT_PATHS_LANE_WIDTH, + BufferID.WAYPOINT_PATHS_SPEED_LIMIT, + } + _road_waypoints_set = { + BufferID.ROAD_WAYPOINTS_POSITION, + BufferID.ROAD_WAYPOINTS_HEADING, + BufferID.ROAD_WAYPOINTS_LANE_ID, + BufferID.ROAD_WAYPOINTS_LANE_INDEX, + BufferID.ROAD_WAYPOINTS_LANE_OFFSET, + BufferID.ROAD_WAYPOINTS_LANE_WIDTH, + BufferID.ROAD_WAYPOINTS_LANE_INDEX, + BufferID.ROAD_WAYPOINTS_SPEED_LIMIT, + } + _via_near_set = { + BufferID.VIA_DATA_NEAR_VIA_POINTS_HIT, + BufferID.VIA_DATA_NEAR_VIA_POINTS_LANE_INDEX, + BufferID.VIA_DATA_NEAR_VIA_POINTS_POSITION, + BufferID.VIA_DATA_NEAR_VIA_POINTS_REQUIRED_SPEED, + BufferID.VIA_DATA_NEAR_VIA_POINTS_ROAD_ID, + } + _lidar_set = { + BufferID.LIDAR_POINT_CLOUD_DIRECTION, + BufferID.LIDAR_POINT_CLOUD_HITS, + BufferID.LIDAR_POINT_CLOUD_ORIGIN, + BufferID.LIDAR_POINT_CLOUD_POINTS, + } + _signals_set = { + # BufferID.SIGNALS_CONTROLLED_LANES, + BufferID.SIGNALS_LAST_CHANGED, + BufferID.SIGNALS_LIGHT_STATE, + BufferID.SIGNALS_STOP_POINT, + } + + def __init__(self) -> None: + self._memos = {} + + def should_get_data(self, buffer_id: BufferID, observation: Observation): + """If the buffer can and should get data from the observation.""" + if ( + buffer_id in self._acceleration_set + and observation.ego_vehicle_state.linear_acceleration is None + ): + return False + elif ( + buffer_id in self._jerk_set + and observation.ego_vehicle_state.linear_jerk is None + ): + return False + elif buffer_id in self._waypoints_set and ( + observation.waypoint_paths is None or len(observation.waypoint_paths) == 0 + ): + return False + elif buffer_id in self._road_waypoints_set and ( + observation.road_waypoints is None + or len(observation.road_waypoints.lanes) == 0 + ): + return False + elif ( + buffer_id in self._via_near_set + and len(observation.via_data.near_via_points) == 0 + ): + return False + elif buffer_id in self._signals_set and ( + observation.signals is None or len(observation.signals) > 0 + ): + return False + elif buffer_id in self._lidar_set and (observation.lidar_point_cloud is None): + return False + + return True + + def get_data_for_buffer(self, buffer_id: BufferID, observation: Observation): + """Retrieve the data buffer from the observation.""" + if len(self._static_methods) == 0: + self._gen_methods_for_buffer() + return self._static_methods.get(buffer_id, lambda o, m: None)( + observation, self._get_memo_for_buffer(buffer_id) + ) + + def _get_memo_for_buffer(self, buffer_id: BufferID): + if buffer_id in self._waypoints_set: + return self._waypoint_paths_flattened + elif buffer_id in self._road_waypoints_set: + return self._road_waypoints_flattened + return None + + @lru_cache(maxsize=1) + def _road_waypoints_flattened(self, o: Observation): + return [ + wp + for wp in itertools.chain( + *(wpl for wpl in itertools.chain(*o.road_waypoints.lanes.values())) + ) + ] + + @lru_cache(maxsize=1) + def _waypoint_paths_flattened(self, o: Observation): + return [wp for wp in itertools.chain(*o.waypoint_paths)] + + @classmethod + def _gen_methods_for_buffer(cls): + cls._static_methods[BufferID.DELTA_TIME] = lambda o, m: o.dt + cls._static_methods[BufferID.ELAPSED_SIM_TIME] = lambda o, m: o.elapsed_sim_time + cls._static_methods[BufferID.EGO_VEHICLE_STATE_HEADING] = lambda o, m: float( + o.ego_vehicle_state.heading + ) + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_SPEED + ] = lambda o, m: o.ego_vehicle_state.speed + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_YAW_RATE + ] = lambda o, m: o.ego_vehicle_state.yaw_rate + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_STEERING + ] = lambda o, m: o.ego_vehicle_state.steering + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_LANE_INDEX + ] = lambda o, m: o.ego_vehicle_state.yaw_rate + cls._static_methods[ + BufferID.DISTANCE_TRAVELLED + ] = lambda o, m: o.distance_travelled + + cls._static_methods[BufferID.STEP_COUNT] = lambda o, m: o.step_count + cls._static_methods[BufferID.STEPS_COMPLETED] = lambda o, m: o.steps_completed + cls._static_methods[BufferID.VEHICLE_TYPE] = ( + lambda o, m: hash(o.ego_vehicle_state.mission.vehicle_spec.veh_config_type) + if o.ego_vehicle_state.mission.vehicle_spec + else -1 + ) + + cls._static_methods[BufferID.EVENTS_COLLISIONS] = lambda o, m: len( + o.events.collisions + ) + cls._static_methods[BufferID.EVENTS_OFF_ROAD] = lambda o, m: int( + o.events.off_road + ) + cls._static_methods[BufferID.EVENTS_OFF_ROUTE] = lambda o, m: int( + o.events.off_route + ) + cls._static_methods[BufferID.EVENTS_ON_SHOULDER] = lambda o, m: int( + o.events.on_shoulder + ) + cls._static_methods[BufferID.EVENTS_WRONG_WAY] = lambda o, m: int( + o.events.wrong_way + ) + cls._static_methods[BufferID.EVENTS_NOT_MOVING] = lambda o, m: int( + o.events.not_moving + ) + cls._static_methods[BufferID.EVENTS_REACHED_GOAL] = lambda o, m: int( + o.events.reached_goal + ) + cls._static_methods[ + BufferID.EVENTS_REACHED_MAX_EPISODE_STEPS + ] = lambda o, m: int(o.events.reached_max_episode_steps) + cls._static_methods[BufferID.EVENTS_AGENTS_ALIVE_DONE] = lambda o, m: int( + o.events.agents_alive_done + ) + cls._static_methods[BufferID.EVENTS_INTEREST_DONE] = lambda o, m: int( + o.events.interest_done + ) + cls._static_methods[BufferID.UNDER_THIS_VEHICLE_CONTROL] = lambda o, m: int( + o.under_this_agent_control + ) + + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_POSITION + ] = lambda o, m: o.ego_vehicle_state.position + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_BOUNDING_BOX + ] = lambda o, m: o.ego_vehicle_state.bounding_box.as_lwh + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_LANE_POSITION + ] = lambda o, m: tuple(o.ego_vehicle_state.lane_position) + + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_LINEAR_VELOCITY + ] = lambda o, m: o.ego_vehicle_state.linear_velocity + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_ANGULAR_VELOCITY + ] = lambda o, m: o.ego_vehicle_state.angular_velocity + + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_LINEAR_ACCELERATION + ] = lambda o, m: o.ego_vehicle_state.linear_acceleration + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_ANGULAR_ACCELERATION + ] = lambda o, m: o.ego_vehicle_state.angular_acceleration + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_LINEAR_JERK + ] = lambda o, m: o.ego_vehicle_state.linear_jerk + cls._static_methods[ + BufferID.EGO_VEHICLE_STATE_ANGULAR_JERK + ] = lambda o, m: o.ego_vehicle_state.angular_jerk + + cls._static_methods[BufferID.EGO_VEHICLE_STATE_ROAD_ID] = lambda o, m: hash( + o.ego_vehicle_state.road_id + ) + cls._static_methods[BufferID.EGO_VEHICLE_STATE_LANE_ID] = lambda o, m: hash( + o.ego_vehicle_state.lane_id + ) + + # XXX: Float cast is sometimes needed because Panda3D reacts badly to non-standard types like numpy float64. + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_POSITION + ] = lambda o, m: [vs.position for vs in o.neighborhood_vehicle_states] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_BOUNDING_BOX + ] = lambda o, m: [ + vs.bounding_box.as_lwh for vs in o.neighborhood_vehicle_states + ] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_POSITION + ] = lambda o, m: [ + tuple(float(v) for v in vs.lane_position) + if vs.lane_position is not None + else (-1.0, -1.0, -1.0) + for vs in o.neighborhood_vehicle_states + ] + + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_HEADING + ] = lambda o, m: [float(vs.heading) for vs in o.neighborhood_vehicle_states] + cls._static_methods[BufferID.NEIGHBORHOOD_VEHICLE_STATES_SPEED] = lambda o, m: [ + vs.speed for vs in o.neighborhood_vehicle_states + ] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_ROAD_ID + ] = lambda o, m: [hash(vs.road_id) for vs in o.neighborhood_vehicle_states] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_ID + ] = lambda o, m: [hash(vs.lane_id) for vs in o.neighborhood_vehicle_states] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_INDEX + ] = lambda o, m: [vs.lane_index for vs in o.neighborhood_vehicle_states] + cls._static_methods[ + BufferID.NEIGHBORHOOD_VEHICLE_STATES_INTEREST + ] = lambda o, m: [int(vs.interest) for vs in o.neighborhood_vehicle_states] + + cls._static_methods[BufferID.WAYPOINT_PATHS_POSITION] = lambda o, m: [ + wp.position.tolist() for wp in m(o) + ] + cls._static_methods[BufferID.WAYPOINT_PATHS_HEADING] = lambda o, m: [ + float(wp.heading) for wp in m(o) + ] + cls._static_methods[BufferID.WAYPOINT_PATHS_LANE_WIDTH] = lambda o, m: [ + wp.lane_width for wp in m(o) + ] + cls._static_methods[BufferID.WAYPOINT_PATHS_SPEED_LIMIT] = lambda o, m: [ + wp.speed_limit for wp in m(o) + ] + cls._static_methods[BufferID.WAYPOINT_PATHS_LANE_OFFSET] = lambda o, m: [ + wp.lane_offset for wp in m(o) + ] + + cls._static_methods[BufferID.WAYPOINT_PATHS_LANE_ID] = lambda o, m: [ + hash(wp.lane_id) for wp in m(o) + ] + cls._static_methods[BufferID.WAYPOINT_PATHS_LANE_INDEX] = lambda o, m: [ + wp.lane_index for wp in m(o) + ] + + cls._static_methods[BufferID.ROAD_WAYPOINTS_POSITION] = lambda o, m: [ + wp.position.tolist() for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_HEADING] = lambda o, m: [ + float(wp.heading) for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_LANE_WIDTH] = lambda o, m: [ + wp.lane_width for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_SPEED_LIMIT] = lambda o, m: [ + wp.speed_limit for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_LANE_OFFSET] = lambda o, m: [ + wp.lane_offset for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_LANE_ID] = lambda o, m: [ + hash(wp.lane_id) for wp in m(o) + ] + cls._static_methods[BufferID.ROAD_WAYPOINTS_LANE_INDEX] = lambda o, m: [ + wp.lane_index for wp in m(o) + ] + + cls._static_methods[BufferID.VIA_DATA_NEAR_VIA_POINTS_POSITION] = lambda o, m: [ + via.position for via in o.via_data.near_via_points + ] + cls._static_methods[ + BufferID.VIA_DATA_NEAR_VIA_POINTS_LANE_INDEX + ] = lambda o, m: [via.lane_index for via in o.via_data.near_via_points] + cls._static_methods[BufferID.VIA_DATA_NEAR_VIA_POINTS_ROAD_ID] = lambda o, m: [ + hash(via.road_id) for via in o.via_data.near_via_points + ] + cls._static_methods[BufferID.VIA_DATA_NEAR_VIA_POINTS_HIT] = lambda o, m: [ + int(via.hit) for via in o.via_data.near_via_points + ] + cls._static_methods[ + BufferID.VIA_DATA_NEAR_VIA_POINTS_REQUIRED_SPEED + ] = lambda o, m: [via.required_speed for via in o.via_data.near_via_points] + + cls._static_methods[BufferID.LIDAR_POINT_CLOUD_POINTS] = lambda o, m: [ + l.tolist() for l in o.lidar_point_cloud[0] + ] + cls._static_methods[BufferID.LIDAR_POINT_CLOUD_HITS] = lambda o, m: [ + int(h) for h in o.lidar_point_cloud[1] + ] + cls._static_methods[BufferID.LIDAR_POINT_CLOUD_ORIGIN] = lambda o, m: [ + o.tolist() for o, _ in o.lidar_point_cloud[2] + ] + cls._static_methods[BufferID.LIDAR_POINT_CLOUD_DIRECTION] = lambda o, m: [ + d.tolist() for _, d in o.lidar_point_cloud[2] + ] + + cls._static_methods[BufferID.SIGNALS_LIGHT_STATE] = lambda o, m: [ + int(l.state) for l in o.signals + ] + cls._static_methods[BufferID.SIGNALS_STOP_POINT] = lambda o, m: [ + tuple(l.stop_point) for l in o.signals + ] + cls._static_methods[BufferID.SIGNALS_LAST_CHANGED] = lambda o, m: [ + l.last_changed for l in o.signals + ] + + +@dataclass +class P3DShaderStep(_P3DCameraMixin, ShaderStep): + """A camera used for rendering images using a shader and a full-screen quad.""" + + fullscreen_quad_node: NodePath + + def update( + self, + pose: Optional[Pose] = None, + height: Optional[float] = None, + observation: Optional[Observation] = None, + **kwargs, + ): + """Update the location of the shader directional values. + Args: + pose: + The pose of the camera target. + height: + The height of the camera above the camera target. + """ + inputs = {} + if pose is not None: + self.fullscreen_quad_node.setShaderInputs( + iHeading=pose.heading, + iTranslation=(pose.point.x, pose.point.y), + ) + inputs["iHeading"] = pose.heading + inputs["iTranslation"] = (pose.point.x, pose.point.y) + if height is not None: + inputs["iElevation"] = height + if len(self.buffer_dependencies) == 0: + return + + buffers = set(self.buffer_dependencies) + if observation is not None: + ba = _BufferAccessor() + for b in buffers: + if ba.should_get_data(b.buffer_id, observation): + inputs[b.script_uniform_name] = ba.get_data_for_buffer( + b.buffer_id, observation + ) + if inputs: + self.fullscreen_quad_node.setShaderInputs(**inputs) + + @property + def position(self) -> Tuple[float, float, float]: + raise ValueError("Shader step does not have a position.") class Renderer(RendererBase): @@ -287,7 +721,7 @@ def __init__( self._dashed_lines_np = None self._vehicle_nodes = {} self._signal_nodes = {} - self._camera_nodes = {} + self._camera_nodes: Dict[str, Union[P3DOffscreenCamera, P3DShaderStep]] = {} _ShowBaseInstance.set_rendering_verbosity(debug_mode=debug_mode) _ShowBaseInstance.set_rendering_backend(rendering_backend=rendering_backend) # Note: Each instance of the SMARTS simulation will have its own Renderer, @@ -346,20 +780,24 @@ def _load_line_data(self, path: Path, name: str) -> GeomNode: geom = Geom(vdata) geom.addPrimitive(prim) - np = GeomNode(name) - np.addGeom(geom) - return np - - def setup(self, scenario: Scenario): - """Initialize this renderer.""" - self._root_np = self._showbase_instance.setup_sim_root(self._simid) - self._vehicles_np = self._root_np.attachNewNode("vehicles") - self._signals_np = self._root_np.attachNewNode("signals") - - map_path = scenario.map_glb_filepath - map_dir = Path(map_path).parent + node_path = GeomNode(name) + node_path.addGeom(geom) + return node_path + + def _ensure_root(self): + if self._root_np is None: + self._root_np = self._showbase_instance.setup_sim_root(self._simid) + # if self._log.getEffectiveLevel() <= logging.DEBUG: + # self._log.debug( + print( + "Renderer started with backend %s", + self._showbase_instance.pipe.get_type(), + ) + def load_road_map(self, map_path: Union[str, Path]): + """Load the road map from its path.""" # Load map + self._ensure_root() if self._road_map_np: self._log.debug( "road_map=%s already exists. Removing and adding a new " @@ -367,13 +805,27 @@ def setup(self, scenario: Scenario): self._road_map_np, map_path, ) - map_np = self._showbase_instance.loader.loadModel(map_path, noCache=True) - np = self._root_np.attachNewNode("road_map") - map_np.reparent_to(np) - np.hide(RenderMasks.OCCUPANCY_HIDE) - np.setColor(SceneColors.Road.value) - self._road_map_np = np + with suppress_output(): + map_np = self._showbase_instance.loader.loadModel(map_path, noCache=True) + node_path = self._root_np.attachNewNode("road_map") + map_np.reparent_to(node_path) + node_path.hide(RenderMasks.OCCUPANCY_HIDE) + node_path.setColor(SceneColors.Road.value) + self._road_map_np = node_path + self._is_setup = True + return map_np.getBounds() + def setup(self, scenario: StaticScenario): + """Initialize this renderer.""" + self._ensure_root() + self._vehicles_np = self._root_np.attachNewNode("vehicles") + self._signals_np = self._root_np.attachNewNode("signals") + + map_path = scenario.map_glb_filepath + map_dir = Path(map_path).parent + + # Load map + self.load_road_map(map_path) # Road lines (solid, yellow) road_lines_path = map_dir / "road_lines.glb" if road_lines_path.exists(): @@ -382,7 +834,6 @@ def setup(self, scenario: Scenario): solid_lines_np.setColor(SceneColors.EdgeDivider.value) solid_lines_np.hide(RenderMasks.OCCUPANCY_HIDE) solid_lines_np.setRenderModeThickness(2) - # Lane lines (dashed, white) lane_lines_path = map_dir / "lane_lines.glb" if lane_lines_path.exists(): @@ -403,7 +854,7 @@ def setup(self, scenario: Scenario): ) dashed_lines_np.setShader(dashed_line_shader, priority=20) dashed_lines_np.setShaderInput( - "Resolution", self._showbase_instance.getSize() + "iResolution", self._showbase_instance.getSize() ) self._dashed_lines_np = dashed_lines_np if scenario_metadata := scenario.metadata: @@ -418,15 +869,22 @@ def setup(self, scenario: Scenario): def render(self): """Render the scene graph of the simulation.""" - assert self._is_setup + if not self._is_setup: + self._ensure_root() + warnings.warn( + "Renderer is not setup. Rendering before scene setup may be unintentional.", + RendererNotSetUpWarning, + ) self._showbase_instance.render_node(self._root_np) def reset(self): """Reset the render back to initialized state.""" - self._vehicles_np.removeNode() - self._vehicles_np = self._root_np.attachNewNode("vehicles") - self._signals_np.removeNode() - self._signals_np = self._root_np.attachNewNode("signals") + if self._vehicles_np is not None: + self._vehicles_np.removeNode() + self._vehicles_np = self._root_np.attachNewNode("vehicles") + if self._signals_np is not None: + self._signals_np.removeNode() + self._signals_np = self._root_np.attachNewNode("signals") self._vehicle_nodes = {} self._signal_nodes = {} @@ -434,7 +892,7 @@ def step(self): """provided for non-SMARTS uses; normally not used by SMARTS.""" self._showbase_instance.taskMgr.step() - def sync(self, sim_frame): + def sync(self, sim_frame: SimulationFrame): """Update the current state of the vehicles and signals within the renderer.""" signal_ids = set() for actor_id, actor_state in sim_frame.actor_states_by_id.items(): @@ -513,6 +971,8 @@ def create_vehicle_node( pos, heading = pose.as_panda3d() node_path.setPosHpr(*pos, heading, 0, 0) node_path.hide(RenderMasks.DRIVABLE_AREA_HIDE) + if color in (SceneColors.Agent,): + node_path.hide(RenderMasks.OCCUPANCY_HIDE) self._vehicle_nodes[vid] = node_path return True @@ -576,13 +1036,13 @@ def create_signal_node( geom_node = GeomNode(name) geom_node.addGeom(geom) - np = self._root_np.attachNewNode(geom_node) - np.setName(name) - np.setColor(color.value) - np.setPos(position.x, position.y, 0.01) - np.setScale(0.9, 0.9, 1) - np.hide(RenderMasks.DRIVABLE_AREA_HIDE) - self._signal_nodes[sig_id] = np + node_path = self._root_np.attachNewNode(geom_node) + node_path.setName(name) + node_path.setColor(color.value) + node_path.setPos(position.x, position.y, 0.01) + node_path.setScale(0.9, 0.9, 1) + node_path.hide(RenderMasks.DRIVABLE_AREA_HIDE) + self._signal_nodes[sig_id] = node_path return True def begin_rendering_signal(self, sig_id: str): @@ -613,7 +1073,7 @@ def remove_signal_node(self, sig_id: str): signal_np.removeNode() del self._signal_nodes[sig_id] - def camera_for_id(self, camera_id) -> P3dOffscreenCamera: + def camera_for_id(self, camera_id: str) -> Union[P3DOffscreenCamera, P3DShaderStep]: """Get a camera by its id.""" camera = self._camera_nodes.get(camera_id) assert ( @@ -628,7 +1088,7 @@ def build_offscreen_camera( width: int, height: int, resolution: float, - ) -> str: + ) -> None: """Generates a new off-screen camera.""" # setup buffer win_props = WindowProperties.size(width, height) @@ -654,7 +1114,7 @@ def build_offscreen_camera( # Necessary for the lane lines to be in the proper proportions if self._dashed_lines_np is not None: self._dashed_lines_np.setShaderInput( - "Resolution", (buffer.size.x, buffer.size.y) + "iResolution", (buffer.size.x, buffer.size.y) ) # setup texture @@ -676,7 +1136,306 @@ def build_offscreen_camera( # mask is set to make undesirable objects invisible to this camera camera_np.node().setCameraMask(camera_np.node().getCameraMask() & mask) - camera = P3dOffscreenCamera(self, camera_np, buffer, tex) + camera = P3DOffscreenCamera(self, camera_np, buffer, tex) self._camera_nodes[name] = camera - return name + def build_shader_step( + self, + name: str, + fshader_path: Union[str, Path], + dependencies: Collection[ + ShaderStepDependencyBase # Union[ShaderStepCameraDependency, ShaderStepVariableDependency, ShaderStepBufferDependency] + ], + priority: int, + height: int, + width: int, + ) -> None: + # setup buffer + win_props = WindowProperties.size(width, height) + fb_props = FrameBufferProperties() + fb_props.setRgbColor(True) + fb_props.setRgbaBits(8, 8, 8, 0) + # XXX: Though we don't need the depth buffer returned, setting this to 0 + # causes undefined behavior where the ordering of meshes is random. + fb_props.setDepthBits(0) + + buffer = self._showbase_instance.win.engine.makeOutput( + self._showbase_instance.pipe, + "{}-buffer".format(name), + priority, + fb_props, + win_props, + GraphicsPipe.BFRefuseWindow, + self._showbase_instance.win.getGsg(), + self._showbase_instance.win, + ) + + cm = CardMaker("filter-stage-quad") + cm.setFrameFullscreenQuad() + quad = NodePath(cm.generate()) + quad.setDepthTest(0) + quad.setDepthWrite(0) + quad.setColor(1, 0.5, 0.5, 1) + + # setup texture + tex = Texture() + # tex.setup_2d_texture(width, height, Texture.T_unsigned_byte, Texture.F_r8i) + region = buffer.getDisplayRegion(0) + region.window.addRenderTexture( + tex, GraphicsOutput.RTM_copy_ram, GraphicsOutput.RTP_color + ) + + # setup camera + lens = OrthographicLens() + lens.setFilmSize(width, height) + lens.setFilmSize(2, 2) + lens.setFilmOffset(0, 0) + lens.setNearFar(-1000, 1000) + + quadcamnode = Camera(name) + quadcamnode.setLens(lens) + quadcam: NodePath = quad.attachNewNode(quadcamnode) + + dr = buffer.makeDisplayRegion((0, 1, 0, 1)) + dr.disableClears() + dr.setCamera(quadcam) + dr.setActive(True) + dr.setScissorEnabled(False) + + # buffer clearing + buffer.setClearColor((0, 0, 0, 0)) # Set background color to black + buffer.setClearColorActive(True) + + assert tex.getExpectedRamImageSize() == tex.getXSize() * tex.getYSize() * 3 + + with pkg_resources.path(glsl, "unlit_shader.vert") as vshader_path: + quad.setShader( + Shader.load(Shader.SL_GLSL, vertex=vshader_path, fragment=fshader_path) + ) + camera_dependencies = tuple( + c for c in dependencies if isinstance(c, ShaderStepCameraDependency) + ) + cameras = tuple( + self.camera_for_id(c.camera_id) + for c in camera_dependencies + if isinstance(c, ShaderStepCameraDependency) + ) + for dep, dep_cam in zip(camera_dependencies, cameras): + quad.setShaderInput(dep.script_variable_name, dep_cam.tex) + size_x, size_y = dep_cam.image_dimensions + quad.setShaderInput( + f"{dep.script_variable_name}Resolution", n1=size_x, n2=size_y + ) + buffer_dependencies = tuple( + v for v in dependencies if isinstance(v, ShaderStepBufferDependency) + ) + + Renderer._set_shader_inputs( + quad, width, height, buffers=buffer_dependencies + ) + variable_dependencies = tuple( + v for v in dependencies if isinstance(v, ShaderStepVariableDependency) + ) + for dep in variable_dependencies: + shader_input = ShaderInput(dep.script_variable_name, dep.value) + quad.setShaderInput(shader_input) + + camera = P3DShaderStep( + self, + shader_file=fshader_path, + camera_dependencies=cameras, + buffer_dependencies=buffer_dependencies, + camera_np=quadcam, + buffer=buffer, + tex=tex, + fullscreen_quad_node=quad, + ) + self._camera_nodes[name] = camera + + @staticmethod + def _set_shader_inputs( + surface, width, height, buffers: Tuple[ShaderStepBufferDependency] + ): + + inputs = { + "iResolution": (width, height), + "iTranslation": (0.0, 0.0), + "iHeading": 0.0, + "iElevation": 0.0, + } + + for svn, bn in ((b.script_uniform_name, b.buffer_id) for b in buffers): + initial_value = None + # SINGLE VALUES + if bn in ( + BufferID.DELTA_TIME, + BufferID.ELAPSED_SIM_TIME, + BufferID.EGO_VEHICLE_STATE_HEADING, + BufferID.EGO_VEHICLE_STATE_SPEED, + BufferID.EGO_VEHICLE_STATE_STEERING, + BufferID.EGO_VEHICLE_STATE_YAW_RATE, + BufferID.DISTANCE_TRAVELLED, + ): + initial_value = float() + elif bn in ( + BufferID.STEP_COUNT, + BufferID.STEPS_COMPLETED, + BufferID.VEHICLE_TYPE, + ): + initial_value = int() + elif bn in ( + BufferID.EVENTS_COLLISIONS, + BufferID.EVENTS_OFF_ROAD, + BufferID.EVENTS_OFF_ROUTE, + BufferID.EVENTS_ON_SHOULDER, + BufferID.EVENTS_WRONG_WAY, + BufferID.EVENTS_NOT_MOVING, + BufferID.EVENTS_REACHED_GOAL, + BufferID.EVENTS_REACHED_MAX_EPISODE_STEPS, + BufferID.EVENTS_AGENTS_ALIVE_DONE, + BufferID.EVENTS_INTEREST_DONE, + BufferID.UNDER_THIS_VEHICLE_CONTROL, + ): + initial_value = bool() + elif bn in ( + BufferID.EGO_VEHICLE_STATE_POSITION, + BufferID.EGO_VEHICLE_STATE_BOUNDING_BOX, + BufferID.EGO_VEHICLE_STATE_LANE_POSITION, + ): + initial_value = (float(), float(), float()) + elif bn in ( + BufferID.EGO_VEHICLE_STATE_LINEAR_VELOCITY, + BufferID.EGO_VEHICLE_STATE_ANGULAR_VELOCITY, + BufferID.EGO_VEHICLE_STATE_LINEAR_ACCELERATION, + BufferID.EGO_VEHICLE_STATE_ANGULAR_ACCELERATION, + BufferID.EGO_VEHICLE_STATE_LINEAR_JERK, + BufferID.EGO_VEHICLE_STATE_ANGULAR_JERK, + ): + initial_value = (float(), float()) + elif bn in ( + BufferID.EGO_VEHICLE_STATE_ROAD_ID, + BufferID.EGO_VEHICLE_STATE_LANE_ID, + BufferID.EGO_VEHICLE_STATE_LANE_INDEX, + ): + initial_value = int() + + # Vector of NEIGHBORHOOD_VEHICLE_STATES + elif bn in ( + BufferID.NEIGHBORHOOD_VEHICLE_STATES_POSITION, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_BOUNDING_BOX, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_POSITION, + ): + initial_value = [ + (float(), float(), float()), + ] * 20 + elif bn in ( + BufferID.NEIGHBORHOOD_VEHICLE_STATES_HEADING, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_SPEED, + ): + initial_value = [ + float(), + ] + elif bn in ( + BufferID.NEIGHBORHOOD_VEHICLE_STATES_ROAD_ID, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_ID, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_LANE_INDEX, + BufferID.NEIGHBORHOOD_VEHICLE_STATES_INTEREST, + ): + initial_value = [ + int(), + ] + + # Vector of waypoints from WAYPOINT_PATHS + elif bn in (BufferID.WAYPOINT_PATHS_POSITION,): + initial_value = [(float(), float())] + elif bn in ( + BufferID.WAYPOINT_PATHS_HEADING, + BufferID.WAYPOINT_PATHS_LANE_WIDTH, + BufferID.WAYPOINT_PATHS_SPEED_LIMIT, + BufferID.WAYPOINT_PATHS_LANE_OFFSET, + ): + initial_value = [ + float(), + ] + elif bn in ( + BufferID.WAYPOINT_PATHS_LANE_ID, + BufferID.WAYPOINT_PATHS_LANE_INDEX, + ): + initial_value = [ + int(), + ] + + # Vector of waypoints from ROAD_WAYPOINTS + elif bn in (BufferID.ROAD_WAYPOINTS_POSITION,): + initial_value = [ + (float(), float()), + ] + elif bn in ( + BufferID.ROAD_WAYPOINTS_HEADING, + BufferID.ROAD_WAYPOINTS_LANE_WIDTH, + BufferID.ROAD_WAYPOINTS_SPEED_LIMIT, + BufferID.ROAD_WAYPOINTS_LANE_OFFSET, + ): + initial_value = [ + float(), + ] + elif bn in ( + BufferID.ROAD_WAYPOINTS_LANE_ID, + BufferID.ROAD_WAYPOINTS_LANE_INDEX, + ): + initial_value = [ + int(), + ] + + # Vector of via data from VIA_DATA + elif bn in (BufferID.VIA_DATA_NEAR_VIA_POINTS_POSITION,): + initial_value = [ + (float(), float()), + ] + elif bn in ( + BufferID.VIA_DATA_NEAR_VIA_POINTS_LANE_INDEX, + BufferID.VIA_DATA_NEAR_VIA_POINTS_ROAD_ID, + BufferID.VIA_DATA_NEAR_VIA_POINTS_HIT, + ): + initial_value = [ + int(), + ] + elif bn in (BufferID.VIA_DATA_NEAR_VIA_POINTS_REQUIRED_SPEED,): + initial_value = [ + float(), + ] + + # Vector of lidar point information from LIDAR_POINT_CLOUD + elif bn in ( + BufferID.LIDAR_POINT_CLOUD_POINTS, + BufferID.LIDAR_POINT_CLOUD_ORIGIN, + BufferID.LIDAR_POINT_CLOUD_DIRECTION, + ): + initial_value = [ + (float(), float(), float()), + ] + elif bn in (BufferID.LIDAR_POINT_CLOUD_HITS,): + initial_value = [ + int(), + ] + + # SIGNALS + elif bn in ( + BufferID.SIGNALS_LIGHT_STATE, + # BufferName.SIGNALS_CONTROLLED_LANES, + ): + initial_value = [ + int(), + ] + elif bn in (BufferID.SIGNALS_STOP_POINT,): + initial_value = [float(), float()] + elif bn in (BufferID.SIGNALS_LAST_CHANGED,): + initial_value = [ + float(), + ] + else: + raise ValueError(f"Buffer `{bn}` is not yet implemented.") + + inputs[svn] = initial_value + + surface.setShaderInputs(**inputs) diff --git a/smarts/p3d/tests/test_p3d_renderer.py b/smarts/p3d/tests/test_p3d_renderer.py index d893cdd3dd..c022a50405 100644 --- a/smarts/p3d/tests/test_p3d_renderer.py +++ b/smarts/p3d/tests/test_p3d_renderer.py @@ -30,7 +30,7 @@ import smarts.assets from smarts.core.colors import SceneColors from smarts.core.coordinates import Heading, Pose -from smarts.core.plan import EndlessGoal, Mission, Start +from smarts.core.plan import EndlessGoal, NavigationMission, Start from smarts.core.scenario import Scenario from smarts.core.utils.custom_exceptions import RendererException @@ -39,7 +39,7 @@ @pytest.fixture def scenario(): - mission = Mission( + mission = NavigationMission( start=Start(np.array((71.65, 63.78)), Heading(math.pi * 0.91)), goal=EndlessGoal(), ) diff --git a/smarts/ray/sensors/ray_sensor_resolver.py b/smarts/ray/sensors/ray_sensor_resolver.py index 333cc59198..0813929845 100644 --- a/smarts/ray/sensors/ray_sensor_resolver.py +++ b/smarts/ray/sensors/ray_sensor_resolver.py @@ -19,22 +19,31 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +from __future__ import annotations + import concurrent.futures import logging from collections import defaultdict -from typing import Optional, Set +from typing import TYPE_CHECKING, Dict, Iterable, Optional, Set, Tuple import ray from smarts.core import config -from smarts.core.configuration import Config from smarts.core.sensors import SensorResolver, Sensors from smarts.core.serialization.default import dumps, loads -from smarts.core.simulation_frame import SimulationFrame -from smarts.core.simulation_local_constants import SimulationLocalConstants from smarts.core.utils.core_logging import timeit from smarts.core.utils.file import replace +if TYPE_CHECKING: + from smarts.core.configuration import Config + from smarts.core.observations import Observation + from smarts.core.renderer_base import RendererBase + from smarts.core.sensor import Sensor + from smarts.core.sensors import SensorState + from smarts.core.simulation_frame import SimulationFrame + from smarts.core.simulation_local_constants import SimulationLocalConstants + from smarts.core.utils.pybullet import bullet_client as bc + logger = logging.getLogger(__name__) @@ -60,8 +69,8 @@ def __init__(self, process_count_override: Optional[int] = None) -> None: if not ray.is_initialized(): ray.init( num_cpus=self._num_observation_workers, - num_gpus=conf("ray", "num_gpus", default=None, cast=int), - log_to_driver=conf("ray", "log_to_driver", default=False, cast=bool), + num_gpus=conf("ray", "num_gpus", cast=int), + log_to_driver=conf("ray", "log_to_driver", cast=bool), ) self._sim_local_constants: SimulationLocalConstants = None self._current_workers = [] @@ -90,17 +99,57 @@ def observe( sim_frame: SimulationFrame, sim_local_constants: SimulationLocalConstants, agent_ids: Set[str], - renderer, - bullet_client, - ): + renderer: RendererBase, + bullet_client: bc.BulletClient, + ) -> Tuple[Dict[str, Observation], Dict[str, bool], Dict[str, Dict[str, Sensor]]]: observations, dones, updated_sensors = {}, {}, defaultdict(dict) ray_actors = self.get_ray_worker_actors(self._num_observation_workers) len_workers = len(ray_actors) + tasks = self._gen_tasks_for_serializable_sensors( + sim_frame, sim_local_constants, agent_ids, ray_actors, len_workers + ) + phys_observations = self._gen_phys_observations( + sim_frame, sim_local_constants, agent_ids, bullet_client, updated_sensors + ) + + # Collect futures + with timeit("waiting for observations", logger.debug): + for fut in concurrent.futures.as_completed( + [task.future() for task in tasks] + ): + obs, ds, u_sens = fut.result() + observations.update(obs) + dones.update(ds) + for v_id, values in u_sens.items(): + updated_sensors[v_id].update(values) + + # Merge physics sensor information + for agent_id, p_obs in phys_observations.items(): + observations[agent_id] = replace(observations[agent_id], **p_obs) + + self._sync_custom_camera_sensors(sim_frame, renderer, observations) + + if renderer: + renderer.render() + + rendering_observations = self._gen_rendered_observations( + sim_frame, sim_local_constants, agent_ids, renderer, updated_sensors + ) + + # Merge sensor information + for agent_id, r_obs in rendering_observations.items(): + observations[agent_id] = replace(observations[agent_id], **r_obs) + + return observations, dones, updated_sensors + + def _gen_tasks_for_serializable_sensors( + self, sim_frame, sim_local_constants, agent_ids, ray_actors, len_workers + ): tasks = [] with timeit( - f"parallizable observations with {len(agent_ids)} and {len(ray_actors)}", + f"setting up parallizable observations with {len(agent_ids)} and {len(ray_actors)}", logger.debug, ): # Update remote state (if necessary) @@ -129,44 +178,9 @@ def observe( ) ) - # While observation processes are operating do rendering - with timeit("rendering", logger.debug): - rendering = {} - for agent_id in agent_ids: - for vehicle_id in sim_frame.vehicles_for_agents[agent_id]: - ( - rendering[agent_id], - updated_unsafe_sensors, - ) = Sensors.process_serialization_unsafe_sensors( - sim_frame, - sim_local_constants, - agent_id, - sim_frame.sensor_states[vehicle_id], - vehicle_id, - renderer, - bullet_client, - ) - updated_sensors[vehicle_id].update(updated_unsafe_sensors) - - # Collect futures - with timeit("waiting for observations", logger.debug): - for fut in concurrent.futures.as_completed( - [task.future() for task in tasks] - ): - obs, ds, u_sens = fut.result() - observations.update(obs) - dones.update(ds) - for v_id, values in u_sens.items(): - updated_sensors[v_id].update(values) - - with timeit("merging observations", logger.debug): - # Merge sensor information - for agent_id, r_obs in rendering.items(): - observations[agent_id] = replace(observations[agent_id], **r_obs) - - return observations, dones, updated_sensors + return tasks - def step(self, sim_frame, sensor_states): + def step(self, sim_frame: SimulationFrame, sensor_states: Iterable[SensorState]): """Step the sensor state.""" for sensor_state in sensor_states: sensor_state.step() @@ -179,7 +193,7 @@ class RayProcessWorker: def __init__(self) -> None: self._simulation_local_constants: Optional[SimulationLocalConstants] = None - def update_local_constants(self, sim_local_constants): + def update_local_constants(self, sim_local_constants: SimulationLocalConstants): """Updates the process worker. Args: @@ -187,7 +201,7 @@ def update_local_constants(self, sim_local_constants): """ self._simulation_local_constants = loads(sim_local_constants) - def do_work(self, remote_sim_frame, agent_ids): + def do_work(self, remote_sim_frame: SimulationFrame, agent_ids: Set[str]): """Run the sensors against the current simulation state. Args: diff --git a/smarts/ray/sensors/tests/test_ray_sensor_resolver.py b/smarts/ray/sensors/tests/test_ray_sensor_resolver.py index 9068e6d9bf..5874575fea 100644 --- a/smarts/ray/sensors/tests/test_ray_sensor_resolver.py +++ b/smarts/ray/sensors/tests/test_ray_sensor_resolver.py @@ -23,7 +23,7 @@ from smarts.core.agent_interface import AgentInterface, AgentType from smarts.core.controllers.action_space_type import ActionSpaceType -from smarts.core.plan import Mission +from smarts.core.plan import NavigationMission from smarts.core.scenario import Scenario from smarts.core.sensors.local_sensor_resolver import LocalSensorResolver from smarts.core.simulation_frame import SimulationFrame @@ -52,7 +52,7 @@ def scenario() -> Scenario: ), ) missions = [ - Mission.random_endless_mission( + NavigationMission.random_endless_mission( s.road_map, ) for _ in AGENT_IDS diff --git a/smarts/ros/src/smarts_ros/scripts/ros_driver.py b/smarts/ros/src/smarts_ros/scripts/ros_driver.py index 062bd31443..e57a19e6ac 100755 --- a/smarts/ros/src/smarts_ros/scripts/ros_driver.py +++ b/smarts/ros/src/smarts_ros/scripts/ros_driver.py @@ -45,7 +45,7 @@ from smarts.core.observations import Observation from smarts.core.plan import ( EndlessGoal, - Mission, + NavigationMission, PositionalGoal, Start, VehicleSpec, @@ -327,7 +327,7 @@ def _agent_spec_callback(self, ros_agent_spec: AgentSpec): ) else: goal = EndlessGoal() - mission = Mission( + mission = NavigationMission( start=Start.from_pose(ROSDriver._pose_from_ros(ros_agent_spec.start_pose)), goal=goal, # TODO: how to prevent them from spawning on top of another existing vehicle? (see how it's done in SUMO traffic) diff --git a/smarts/sstudio/genscenario.py b/smarts/sstudio/genscenario.py index 523af61f2a..e6b281d5da 100644 --- a/smarts/sstudio/genscenario.py +++ b/smarts/sstudio/genscenario.py @@ -191,7 +191,7 @@ def gen_scenario( obj_hash = pickle_hash(scenario.map_spec, True) if _needs_build(db_conn, scenario.map_spec, artifact_paths, obj_hash): with timeit("map_spec", logger.info): - gen_map(scenario_dir, scenario.map_spec) + gen_map_spec_artifact(scenario_dir, scenario.map_spec) _update_artifacts(db_conn, artifact_paths, obj_hash) map_spec = scenario.map_spec else: @@ -362,7 +362,9 @@ def gen_scenario( ) -def gen_map(scenario: str, map_spec: sstypes.MapSpec, output_dir: Optional[str] = None): +def gen_map_spec_artifact( + scenario: str, map_spec: sstypes.MapSpec, output_dir: Optional[str] = None +): """Saves a map spec to file.""" _check_if_called_externally() build_dir = os.path.join(scenario, "build") diff --git a/smarts/sstudio/graphics/__init__.py b/smarts/sstudio/graphics/__init__.py new file mode 100644 index 0000000000..2c65923417 --- /dev/null +++ b/smarts/sstudio/graphics/__init__.py @@ -0,0 +1,21 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/smarts/sstudio/graphics/heightfield.py b/smarts/sstudio/graphics/heightfield.py new file mode 100644 index 0000000000..c722c34a9a --- /dev/null +++ b/smarts/sstudio/graphics/heightfield.py @@ -0,0 +1,187 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from __future__ import annotations + +from abc import ABC +from pathlib import Path +from typing import BinaryIO, Callable, Dict, Optional, Tuple, Union + +import numpy as np + + +class HeightField(ABC): + """A utility for working with greyscale values.""" + + def __init__( + self, + data: np.ndarray, + size: Union[Tuple[int, int], np.ndarray], + metadata: Optional[Dict] = None, + ) -> None: + assert isinstance(data, np.ndarray), "Image must be a numpy array." + assert data.dtype == np.uint8 and ( + len(data.shape) == 2 or (len(data.shape) == 3 and data.shape[-1] == 1) + ), f"Image with {data.dtype} and shape {data.shape} is not greyscale format." + if len(data.shape) == 3: + data = np.squeeze(data, axis=2) + self._data = data + self._size = np.array(size, dtype=np.uint64) + self._resolution = np.array(list(reversed(data.shape)), dtype=np.int64) + self._reciprocal_resolution = np.reciprocal(self._resolution) + self._inverse_size = np.reciprocal(self._size, dtype=np.float64) + self._metadata = metadata or {} + + @property + def data(self): + """The raw underlying data.""" + return self._data + + @property + def dtype(self): + """The per element data type.""" + return self._data.dtype + + @property + def size(self): + """The width and height.""" + return self._size + + @property + def resolution(self): + """Resolution of this height field.""" + return self._resolution + + @property + def metadata(self): + """Additional metadata.""" + return self._metadata + + def _check_match(self, other: HeightField): + return np.all(self._resolution == other._resolution) and np.all( + self._size == other._size + ) + + def add(self, other: HeightField) -> HeightField: + """Add by element.""" + assert self._check_match(other) + return HeightField(np.add(self._data, other._data), self._size) + + def subtract(self, other: HeightField) -> HeightField: + """Subtract by element.""" + assert self._check_match(other) + data = np.subtract(self._data, other._data) + return HeightField(data, self.size) + + def scale_by(self, other: HeightField) -> HeightField: + """Scale this height field by another height field.""" + assert self._check_match(other) + inplace_array = np.multiply( + other._data, + np.reciprocal(np.invert(other._data.dtype.type(0)), dtype=np.float64), + ) + np.multiply(self._data, inplace_array, out=inplace_array) + if self.dtype.type in {"u", "i"}: + inplace_array.round(out=inplace_array) + return HeightField(inplace_array.astype(self.dtype), self.size) + + def multiply(self, other: HeightField) -> HeightField: + """Multiply the byte values between these height fields""" + assert self._check_match(other) + return HeightField(np.multiply(self._data, other._data), self.size) + + def max(self, other: HeightField) -> HeightField: + """Get the maximum value of overlapping height fields.""" + assert self._check_match(other) + return HeightField(np.max([self._data, other._data], axis=0), self.size) + + def inverted(self) -> HeightField: + """Invert this height field assuming 8 bit.""" + data = np.invert(self._data) + return HeightField(data, self._size, self._metadata) + + def apply_kernel( + self, kernel: np.ndarray, min_val=-np.inf, max_val=np.inf, pad_mode="edge" + ): + """Apply a kernel to the whole height field. + + The kernel can be asymmetric but still needs each dimension to be an odd value. + """ + assert len(kernel.shape) == 2 and np.all( + [k % 2 for k in kernel.shape] + ), "Kernel shape must be 2D and shape dimension values must be odd" + k_height, k_width = kernel.shape + m_height, m_width = self.data.shape + k_size = max(k_height, k_width) + padded = np.pad(self.data, (int(k_size / 2), int(k_size / 2)), mode=pad_mode) + + if k_size > 1: + if k_height == 1: + padded = padded[1:-1, :] + elif k_width == 1: + padded = padded[:, 1:-1] + + # iterate through matrix, apply kernel, and sum + output = np.empty_like(self.data) + for v in range(m_height): + for u in range(m_width): + between = padded[v : k_height + v, u : k_width + u] * kernel + output[v][u] = min(max(np.sum(between), min_val), max_val) + + return HeightField(output, self.size) + + def apply_function( + self, + fn: Callable[[np.ndarray, int, int], np.uint8], + min_val=-np.inf, + max_val=np.inf, + ): + """Apply a function to each element.""" + output = np.empty_like(self.data) + for i in range(self.data.shape[0]): + for j in range(self.data.shape[1]): + output[i][j] = min(max(fn(self.data, i, j), min_val), max_val) + + return HeightField(output, self.size) + + def write_image(self, file: Union[str, Path, BinaryIO]): + """Write this out to a greyscale image.""" + from PIL import Image + + a = self.data.astype(np.uint8) + im = Image.fromarray(a, "L") + im.save(file) + + @classmethod + def load_image(cls, file: Union[str, Path]): + """Load from any image.""" + from PIL import Image + + with Image.open(file) as im: + data = np.asarray(im) + assert len(data.shape) == 2 + return cls(data, data.shape[:2]) + + @classmethod + def from_rgb(cls, data: np.ndarray): + """Load from an rgb array.""" + d = np.min(data, axis=2) + return HeightField(d, size=(data.shape[1], data.shape[0])) diff --git a/smarts/sstudio/graphics/mesh2bytemap.py b/smarts/sstudio/graphics/mesh2bytemap.py new file mode 100644 index 0000000000..58a1a085a3 --- /dev/null +++ b/smarts/sstudio/graphics/mesh2bytemap.py @@ -0,0 +1,80 @@ +# Copyright (C) 2020. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import argparse +import os + +import numpy as np + +from smarts.core.coordinates import Dimensions, Pose +from smarts.core.sensor import DrivableAreaGridMapSensor +from smarts.core.vehicle_state import VehicleState +from smarts.p3d.renderer import Renderer +from smarts.sstudio.graphics.heightfield import HeightField + + +def generate_bytemap_from_glb_file( + glb_file: str, out_bytemap_file: str, padding: int, inverted=False +): + """Creates a geometry file from a sumo map file.""" + renderer = Renderer("r") + bounds = renderer.load_road_map(glb_file) + size = (*(bounds.max - bounds.min),) + vs = VehicleState( + "a", + pose=Pose(np.array((*bounds.center,)), np.array([0, 0, 0, 1])), + dimensions=Dimensions(1, 1, 1), + ) + camera = DrivableAreaGridMapSensor( + vehicle_state=vs, + width=int(size[0]) + padding, + height=int(size[1]) + padding, + resolution=1, + renderer=renderer, + ) + renderer.render() + image = camera(renderer) + data = image.data + if inverted: + hf = HeightField(data, (1, 1)) + data = hf.inverted().data + + from PIL import Image + + im = Image.fromarray(data.squeeze(), "L") + im.save(out_bytemap_file) + im.close() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + os.path.basename(__file__), + description="Utility to export mesh files to bytemap.", + ) + parser.add_argument("mesh", help="mesh file (*.glb)", type=str) + parser.add_argument("output_path", help="where to write the bytemap file", type=str) + parser.add_argument("--padding", help="the padding pixels", type=int, default=0) + parser.add_argument( + "--inverted", help="invert the map surface", action="store_true" + ) + args = parser.parse_args() + + generate_bytemap_from_glb_file( + args.mesh, args.output_path, padding=args.padding, inverted=args.inverted + ) diff --git a/smarts/sstudio/tests/heightfield.binary b/smarts/sstudio/tests/heightfield.binary new file mode 100644 index 0000000000..de70c0d092 Binary files /dev/null and b/smarts/sstudio/tests/heightfield.binary differ diff --git a/smarts/sstudio/tests/test_generate.py b/smarts/sstudio/tests/test_generate.py index 07b1b4b217..fb3fb65f8c 100644 --- a/smarts/sstudio/tests/test_generate.py +++ b/smarts/sstudio/tests/test_generate.py @@ -27,7 +27,7 @@ import pytest from smarts.core.scenario import Scenario -from smarts.sstudio.genscenario import gen_map, gen_traffic +from smarts.sstudio.genscenario import gen_map_spec_artifact, gen_traffic from smarts.sstudio.sstypes import ( Distribution, Flow, @@ -110,7 +110,7 @@ def _compare_files(file1, file2): def _gen_map_from_spec(scenario_root: str, map_spec: MapSpec): with tempfile.TemporaryDirectory() as temp_dir: - gen_map(scenario_root, map_spec, output_dir=temp_dir) + gen_map_spec_artifact(scenario_root, map_spec, output_dir=temp_dir) found_map_spec = Scenario.discover_map(temp_dir) assert found_map_spec road_map = found_map_spec.builder_fn(found_map_spec) diff --git a/smarts/sstudio/tests/test_heightfield.py b/smarts/sstudio/tests/test_heightfield.py new file mode 100644 index 0000000000..d3101be921 --- /dev/null +++ b/smarts/sstudio/tests/test_heightfield.py @@ -0,0 +1,88 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import os + +import numpy as np +import pytest + +from smarts.sstudio.graphics.heightfield import HeightField + + +@pytest.fixture +def map_data(): + with open(os.path.join(os.path.dirname(__file__), "heightfield.binary"), "rb") as f: + data = np.frombuffer(f.read(), dtype=np.uint8) + return data.reshape((256, 256)) + + +@pytest.fixture +def map_image(): + return np.array([[[0], [1], [0]], [[0], [1], [0]], [[2], [1], [2]]], dtype=np.uint8) + + +@pytest.fixture +def map_image2(): + return np.array( + [ + [[0], [1], [0], [1], [0]], + [[0], [1], [0], [0], [0]], + [[2], [1], [2], [0], [0]], + ], + dtype=np.uint8, + ) + + +@pytest.fixture +def kernel() -> np.ndarray: + return np.array( + [ + [1.0, 1.0, 0.5, 1.0, 1.0], + [1.0, 0.0, 0.0, 0.0, 1.0], + [0.5, 0.0, -80, 0.0, 0.5], + [1.0, 0.0, 0.0, 0.0, 1.0], + [1.0, 0.5, 0.5, 1.0, 1.0], + ], + dtype=np.float64, + ) + + +def test_heightfield_from_map(map_data): + heighfield = HeightField(map_data, (100, 100)) + assert np.all(heighfield.data == map_data) + + +def test_heighfield_from_image(map_image): + heighfield = HeightField(map_image, (100, 100)) + assert np.all(heighfield.data == np.squeeze(map_image, axis=2)) + + +def test_heightfield_kernel(map_data, kernel): + heightfield = HeightField(map_data, (100, 100)) + field = heightfield.apply_kernel(kernel) + assert isinstance(field, HeightField) + + +def test_heightfield_inverted(map_image): + hf = HeightField(data=map_image, size=(3, 3)) + ihf: HeightField = hf.inverted() + + assert np.all(hf.data == ihf.inverted().data) diff --git a/utils/docker/Dockerfile b/utils/docker/Dockerfile index 539c35ad35..b38a17a6af 100644 --- a/utils/docker/Dockerfile +++ b/utils/docker/Dockerfile @@ -1,4 +1,12 @@ -FROM ubuntu:bionic +# Steps to build and push the SMARTS docker image +# ```bash +# $ cd +# export VERSION=v0.5.0 +# $ docker build --no-cache -f ./utils/docker/Dockerfile -t huaweinoah/smarts:$VERSION . +# $ docker login +# $ docker push huaweinoah/smarts:$VERSION +# ``` +FROM ubuntu:focal ARG DEBIAN_FRONTEND=noninteractive @@ -17,10 +25,11 @@ RUN apt-get update --fix-missing && \ add-apt-repository -y ppa:deadsnakes/ppa && \ apt-get update && \ apt-get install -y \ + git \ + libenchant-dev \ libspatialindex-dev \ python3.8 \ python3.8-venv \ - libenchant-dev \ xorg && \ apt-get autoremove -y && \ rm -rf /var/lib/apt/lists/* @@ -28,16 +37,12 @@ RUN apt-get update --fix-missing && \ # Update default python version. RUN update-alternatives --install /usr/bin/python python /usr/bin/python3.8 1 -# Setup virtual environment and install pip. +# Setup python virtual environment and install pip. ENV VIRTUAL_ENV=/opt/.venv RUN python3.8 -m venv $VIRTUAL_ENV ENV PATH="$VIRTUAL_ENV/bin:$PATH" RUN pip install --upgrade pip && pip install wheel==0.38.4 -# Setup SUMO. -RUN pip install eclipse-sumo==1.12.0 -ENV SUMO_HOME $VIRTUAL_ENV/lib64/python3.8/site-packages/sumo - # Install requirements.txt . COPY ./requirements.txt /tmp/requirements.txt RUN pip install --no-cache-dir -r /tmp/requirements.txt @@ -45,11 +50,13 @@ RUN pip install --no-cache-dir -r /tmp/requirements.txt # Copy source files and install. COPY . /src WORKDIR /src -RUN pip install --no-cache-dir -e .[camera_obs,dev,rllib,test,torch,train] && \ +RUN pip install --no-cache-dir -e .[camera-obs,dev,rllib,test,torch,train] && \ cp -r /src/smarts.egg-info /media/smarts.egg-info # For Envision. EXPOSE 8081 +# For centralized TraCI management server +EXPOSE 8097 # Suppress message of missing /dev/input folder and copy smarts.egg-info if not there RUN echo "mkdir -p /dev/input\n" \ diff --git a/utils/docker/Dockerfile.minimal b/utils/docker/Dockerfile.minimal index e42258b39d..8d75d83981 100644 --- a/utils/docker/Dockerfile.minimal +++ b/utils/docker/Dockerfile.minimal @@ -1,13 +1,14 @@ -# Steps to build and push minimal SMARTS docker image +# Steps to build and push the software rendering SMARTS docker image # ```bash # $ cd -# export VERSION=v0.5.0 -# $ docker build --no-cache -f ./utils/docker/Dockerfile.minimal -t huaweinoah/smarts:$VERSION-minimal . -# $ docker login -# $ docker push huaweinoah/smarts:$VERSION-minimal +# export VERSION=v2.0.0 +# export DOCK_TYPE=minimal +# $ docker build --no-cache -f ./utils/docker/Dockerfile.$DOCK_TYPE -t ghcr.io/smarts-project/smarts:$VERSION-$DOCK_TYPE . +# $ # This uses a key for the smarts-project repository +# $ sudo echo $GHCR_SMARTS_PROJECT | docker login ghcr.io --username smarts-project --password-stdin +# $ docker push ghcr.io/smarts-project/smarts:$VERSION-$DOCK_TYPE # ``` - -FROM ubuntu:bionic +FROM ubuntu:focal ARG DEBIAN_FRONTEND=noninteractive @@ -18,11 +19,11 @@ RUN apt-get update --fix-missing && \ add-apt-repository -y ppa:deadsnakes/ppa && \ apt-get update && \ apt-get install -y \ + git \ + libenchant-dev \ libspatialindex-dev \ python3.8 \ python3.8-venv \ - libenchant-dev \ - git \ wget \ xorg && \ apt-get autoremove -y && \ @@ -38,6 +39,8 @@ RUN wget https://bootstrap.pypa.io/get-pip.py -O get-pip.py && \ # For Envision EXPOSE 8081 +# For centralized TraCI management server +EXPOSE 8097 # Suppress message of missing /dev/input folder RUN echo "mkdir -p /dev/input" >> ~/.bashrc diff --git a/utils/docker/Dockerfile.software_render b/utils/docker/Dockerfile.software_render new file mode 100644 index 0000000000..a8b6e8608c --- /dev/null +++ b/utils/docker/Dockerfile.software_render @@ -0,0 +1,67 @@ +# Steps to build and push minimal SMARTS docker image +# ```bash +# $ cd +# export VERSION=v2.0.0 +# export DOCK_TYPE=software_render +# $ docker build --no-cache -f ./utils/docker/Dockerfile.$DOCK_TYPE -t ghcr.io/smarts-project/smarts:$VERSION-$DOCK_TYPE . +# $ # This uses a key for the smarts-project repository +# $ sudo echo $GHCR_SMARTS_PROJECT | docker login ghcr.io --username smarts-project --password-stdin +# $ docker push ghcr.io/smarts-project/smarts:$VERSION-$DOCK_TYPE +# ``` +FROM ubuntu:focal + +ARG DEBIAN_FRONTEND=noninteractive + +# Install libraries +RUN apt-get update --fix-missing && \ + apt-get install -y \ + software-properties-common && \ + add-apt-repository -y ppa:deadsnakes/ppa && \ + apt-get update && \ + apt-get install -y \ + git \ + libenchant-dev \ + libspatialindex-dev \ + python3.8 \ + python3.8-venv \ + mesa-utils \ + wget \ + x11-apps \ + xorg \ + xserver-xorg-video-dummy && \ + apt-get autoremove -y && \ + rm -rf /var/lib/apt/lists/* + +# Set environment variable to enable software rendering +ENV LIBGL_ALWAYS_SOFTWARE=1 + +# Update default python version +RUN update-alternatives --install /usr/bin/python python /usr/bin/python3.8 1 + +# Setup pip +RUN wget https://bootstrap.pypa.io/get-pip.py -O get-pip.py && \ + python get-pip.py && \ + pip install --upgrade pip + +# Get xorg dummy +# Setup DISPLAY +ENV DISPLAY :1 +# VOLUME /tmp/.X11-unix +RUN wget -O /etc/X11/xorg.conf http://xpra.org/xorg.conf && \ + cp /etc/X11/xorg.conf /usr/share/X11/xorg.conf.d/xorg.conf + +# For Envision +EXPOSE 8081 +# For centralized TraCI management server +EXPOSE 8097 + +# TODO: Find a better place to put this (e.g. through systemd). As it stands now ctrl-c +# could close x-server. Even though it's "running in the background". +RUN echo "/usr/bin/Xorg " \ + "-noreset +extension GLX +extension RANDR +extension RENDER" \ + "-logfile ./xdummy.log -config /etc/X11/xorg.conf -novtswitch $DISPLAY &" >> ~/.bashrc + +# Suppress message of missing /dev/input folder +RUN echo "mkdir -p /dev/input" >> ~/.bashrc + +SHELL ["/bin/bash", "-c", "-l"] diff --git a/utils/singularity/smarts.def b/utils/singularity/smarts.def index 4c19251d6f..be489a6972 100644 --- a/utils/singularity/smarts.def +++ b/utils/singularity/smarts.def @@ -47,7 +47,7 @@ From: ubuntu:bionic # Copy source files and install SMARTS cd /src - pip install --no-cache-dir -e .[train,test,dev,camera_obs] + pip install --no-cache-dir -e .[train,test,dev,camera-obs] cp -r /src/smarts.egg-info /media/smarts.egg-info %environment diff --git a/zoo/policies/open-agent/open_agent/agent.py b/zoo/policies/open-agent/open_agent/agent.py index 07237c9a14..c1a2d6d743 100644 --- a/zoo/policies/open-agent/open_agent/agent.py +++ b/zoo/policies/open-agent/open_agent/agent.py @@ -568,7 +568,7 @@ def act(self, obs): # Give the closest SV_N social vehicles to the planner social_vehicles = sorted( obs.neighborhood_vehicle_states, - key=lambda sv: np.linalg.norm(sv.position - ego.position), + key=lambda sv: np.linalg.norm(np.subtract(sv.position, ego.position)), )[: self.SV_N] # repeat the last social vehicle to ensure SV_N social vehicles