diff --git a/.flake8 b/.flake8 new file mode 100644 index 0000000..126637d --- /dev/null +++ b/.flake8 @@ -0,0 +1,15 @@ +[flake8] +max-line-length = 130 +max-complexity = 23 + +ignore = E701, E501, W503 +exclude = + .git + tests/* + examples/* + +per-file-ignores = + lgsvl/__init__.py:F401 + lgsvl/dreamview/__init__.py:F401 + lgsvl/evaluator/__init__.py:F401 + lgsvl/wise/__init__.py:F401 diff --git a/.gitignore b/.gitignore index 69516f1..1abbf76 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ **/__pycache__ *.egg-info +.coverage +htmlcov diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..65833ea --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,12 @@ +image: "python:3.6.13" + +before_script: + - python3 -m pip install -r requirements.txt .\[ci\] + +stages: + - Style Guide Enforcement + +flake8: + stage: Style Guide Enforcement + script: + - python3 -m flake8 diff --git a/LICENSE b/LICENSE index 70a77ed..0c01b02 100644 --- a/LICENSE +++ b/LICENSE @@ -2,14 +2,14 @@ SIMULATOR SOFTWARE LICENSE AGREEMENT DO NOT DOWNLOAD, INSTALL ACCESS, COPY, OR USE ANY PORTION OF THE LICENSED MATERIALS (DEFINED BELOW) UNTIL YOU HAVE READ AND ACCEPTED THE TERMS OF THIS AGREEMENT. BY INSTALLING, COPYING, ACCESSING, OR USING THE LICENSED MATERIALS, YOU ACCEPT AND AGREE TO BE LEGALLY BOUND BY THE TERMS AND CONDITIONS OF THIS AGREEMENT. If You do not agree to be bound by, or the entity for whose benefit You act has not authorized You to accept these terms and conditions, do not install, access, copy, or use the LG Simulator Software and destroy all copies in your possession. -This Software License Agreement (“Agreement”), effective upon your acceptance of this Agreement (“Effective Date”), is between Zenith Electronics, LLC with offices located at 5150 Great America Parkway, Santa Clara, CA 95054 (“LG”) and "You." “You” or “Your” refers to you or your employer or other entity for whose benefit you act, as applicable. +This Software License Agreement (“Agreement”), effective upon your acceptance of this Agreement (“Effective Date”), is between Zenith Electronics, LLC with offices located at 5150 Great America Parkway, Santa Clara, CA 95054 (“LG”) and "You." “You” or “Your” refers to you or your employer or other entity for whose benefit you act, as applicable. -1 PURPOSE; LICENSE GRANT AND RESTRICTIONS. +1 PURPOSE; LICENSE GRANT AND RESTRICTIONS. -1.1 Purpose. This Agreement governs the use of the LG Simulator Software and any accompanying documentation, code, content owned or provided by LG (“LG Content”) and related materials (collectively, the “Licensed Material”) described in more detail in Exhibit A, made available to You by LG. +1.1 Purpose. This Agreement governs the use of the LG Simulator Software and any accompanying documentation, code, content owned or provided by LG (“LG Content”) and related materials (collectively, the “Licensed Material”) described in more detail in Exhibit A, made available to You by LG. -1.2 License Grant. Subject to the terms and conditions of this Agreement, LG grants You a nonexclusive, sublicensable as permitted herein, royalty-free, worldwide, perpetual, irrevocable license for software development and related purposes (the “Purpose”). Any permitted sublicense must be on the same terms and conditions as contained herein. +1.2 License Grant. Subject to the terms and conditions of this Agreement, LG grants You a nonexclusive, sublicensable as permitted herein, royalty-free, worldwide, perpetual, irrevocable license for software development and related purposes (the “Purpose”). Any permitted sublicense must be on the same terms and conditions as contained herein. 1.3 Restrictions. You may not: (a) reverse engineer, decompile, or disassemble the executable or binary builds of Licensed Material, or otherwise attempt to defeat, avoid, by-pass, remove, deactivate or otherwise circumvent any software protection mechanisms in or included with the binary builds of Licensed Material including, without limitation, any such mechanism used to restrict or control the functionality of the binary builds of Licensed Material; (b) use or reproduce the Licensed Material for any reason other than the Purpose; (c) sell or otherwise transfer or make available the Licensed Material, any copies of the Licensed Material, or any information derived from the Licensed Material in any form to any third parties for commercial purposes; or (d) remove or alter any proprietary notices or marks on the Licensed Material. @@ -19,48 +19,48 @@ This Software License Agreement (“Agreement”), effective upon your acceptanc 2 UPDATES. -You acknowledge that LG may update or modify the Licensed Material from time to time and that such updates and modifications may adversely affect the manner in which You access or communicate with the Licensed Material. +You acknowledge that LG may update or modify the Licensed Material from time to time and that such updates and modifications may adversely affect the manner in which You access or communicate with the Licensed Material. -3 TERM; TERMINATION. +3 TERM; TERMINATION. -3.1 Term. This Agreement will be effective as of the Effective Date and will remain effective until terminated as set forth below in Section 3.2. +3.1 Term. This Agreement will be effective as of the Effective Date and will remain effective until terminated as set forth below in Section 3.2. -3.2 Termination. This Agreement will be terminated immediately by LG at any time, by providing written notice to You (such notice may be via email) upon breach of any term or condition of this Agreement. Upon termination, You and any sublicensees will cease access to the Licensed Material, and destroy all copies of the Licensed Material (including all derivatives thereof) You have obtained or created. Sections 4-8 will survive the termination of this Agreement. +3.2 Termination. This Agreement will be terminated immediately by LG at any time, by providing written notice to You (such notice may be via email) upon breach of any term or condition of this Agreement. Upon termination, You and any sublicensees will cease access to the Licensed Material, and destroy all copies of the Licensed Material (including all derivatives thereof) You have obtained or created. Sections 4-8 will survive the termination of this Agreement. -4 OWNERSHIP; FEEDBACK. +4 OWNERSHIP; FEEDBACK. 4.1 Ownership. As between You and LG, LG owns all right, title and interest in and to the Licensed Material (and any LG modifications or enhancements thereof), including but not limited to all intellectual property rights therein. Any rights not expressly granted herein are withheld. As between You and LG, You (or the third party content provider) own all right, title and interest in and to the Simulator Content. Simulator Content includes but is not limited to vehicle models, high definition maps, sensor parameters, and 3D environments. Simulator Content does not include LG Content. -4.2 Feedback. You agree to provide LG with comments concerning the Licensed Material and Your evaluation and use thereof, including bug reports, evaluations, proposed product integrations (and associated metrics and learned knowledge) (“Feedback”). You agree that LG and its designees will be free to copy, modify, create derivative works, publicly display, disclose, distribute, license and sublicense, incorporate and otherwise use the Feedback, including derivative works thereto, for any and all commercial and non-commercial purposes with no obligation of any kind to You. +4.2 Feedback. You agree to provide LG with comments concerning the Licensed Material and Your evaluation and use thereof, including bug reports, evaluations, proposed product integrations (and associated metrics and learned knowledge) (“Feedback”). You agree that LG and its designees will be free to copy, modify, create derivative works, publicly display, disclose, distribute, license and sublicense, incorporate and otherwise use the Feedback, including derivative works thereto, for any and all commercial and non-commercial purposes with no obligation of any kind to You. 4.3 Open Source. You hereby acknowledge that the Licensed Materials may contain Open Source Software. “Open Source Software” means any software or software component, module or package that contains, or is derived in any manner (in whole or in part) from, any software that is distributed as free software, open source software or similar licensing or distribution models. To the extent any such license requires that LG provide You the rights to copy, modify, distribute or otherwise use any Open Source Software that are inconsistent with the limited rights granted to You in this Agreement, then such rights in the applicable Open Source Software license shall take precedence over the rights and restrictions granted in this Agreement, but solely with respect to such Open Source Software. You agree to review any documentation that accompanies the Licensed Materials or is identified in a link provided in the documentation for the Licensed Materials in order to determine which portions of the Licensed Materials are Open Source Software and are licensed under an Open Source Software license. You acknowledge that the Open Source Software license is solely between You and the applicable licensor of the Open Source Software. You shall comply with the terms of all applicable Open Source Software licenses, if any. -5 REPRESENTATIONS AND WARRANTIES; DISCLAIMER. +5 REPRESENTATIONS AND WARRANTIES; DISCLAIMER. 5.1 Representations and Warranties. You represent and warrant that: (a) the performance of Your obligations will not constitute a breach or otherwise violate any other agreement or the rights of any third party arising therefrom; and (b) You will maintain throughout the Term all rights and licenses that are required with respect to use of the Licensed Material, and Your use does and will continue to comply with all applicable foreign, federal, state, and local laws, rules and regulations. -5.2 Disclaimer. THE LICENSED MATERIAL AND ANY OTHER LG PRODUCTS AND SERVICES PROVIDED HEREUNDER ARE PROVIDED “AS IS” AND WITHOUT WARRANTY OF ANY KIND. LG DISCLAIMS ALL WARRANTIES, WHETHER EXPRESS, IMPLIED, STATUTORY OR OTHERWISE, INCLUDING WITHOUT LIMITATION WARRANTIES OF MERCHANTABILITY, NONINFRINGEMENT, FITNESS FOR A PARTICULAR PURPOSE, AND ANY WARRANTIES OR CONDITIONS ARISING OUT OF COURSE OF DEALING OR USAGE OF TRADE. LG DOES NOT WARRANT THAT THE LICENSED MATERIAL AND ANY OTHER LG PRODUCTS AND SERVICES PROVIDED HEREUNDER WILL MEET ALL OF YOUR REQUIREMENTS OR EXPECTATIONS, OR THAT USE OF SUCH LICENSED MATERIAL BE ERROR-FREE, UNINTERRUPTED, VIRUS-FREE, OR SECURE. +5.2 Disclaimer. THE LICENSED MATERIAL AND ANY OTHER LG PRODUCTS AND SERVICES PROVIDED HEREUNDER ARE PROVIDED “AS IS” AND WITHOUT WARRANTY OF ANY KIND. LG DISCLAIMS ALL WARRANTIES, WHETHER EXPRESS, IMPLIED, STATUTORY OR OTHERWISE, INCLUDING WITHOUT LIMITATION WARRANTIES OF MERCHANTABILITY, NONINFRINGEMENT, FITNESS FOR A PARTICULAR PURPOSE, AND ANY WARRANTIES OR CONDITIONS ARISING OUT OF COURSE OF DEALING OR USAGE OF TRADE. LG DOES NOT WARRANT THAT THE LICENSED MATERIAL AND ANY OTHER LG PRODUCTS AND SERVICES PROVIDED HEREUNDER WILL MEET ALL OF YOUR REQUIREMENTS OR EXPECTATIONS, OR THAT USE OF SUCH LICENSED MATERIAL BE ERROR-FREE, UNINTERRUPTED, VIRUS-FREE, OR SECURE. -6 LIMITATION OF LIABILITY. +6 LIMITATION OF LIABILITY. IN NO EVENT SHALL LG BE LIABLE TO YOU OR YOUR SUBLICENSEES FOR ANY SPECIAL, INCIDENTAL, EXEMPLARY, PUNITIVE OR CONSEQUENTIAL DAMAGES (INCLUDING LOSS OF USE, DATA, BUSINESS OR PROFITS) ARISING OUT OF OR IN CONNECTION WITH THIS AGREEMENT, WHETHER SUCH LIABILITY ARISES FROM ANY CLAIM BASED UPON CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE), STRICT LIABILITY OR OTHERWISE, AND WHETHER OR NOT LG HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH LOSS OR DAMAGE. THE FOREGOING LIMITATIONS WILL SURVIVE AND APPLY EVEN IF ANY LIMITED REMEDY SPECIFIED IN THIS AGREEMENT IS FOUND TO HAVE FAILED OF ITS ESSENTIAL PURPOSE. IN ANY CASE, LG’S AGGREGATE LIABILITY UNDER THIS AGREEMENT WILL NOT EXCEED THE FEE, IF ANY, THAT YOU PAID LG FOR USE OF THE LICENSED MATERIAL. -7 INDEMNIFICATION. +7 INDEMNIFICATION. You will indemnify, defend, or at its option settle and hold LG, its subsidiaries, affiliates, officers and employees, harmless from any and all claims, damages, losses, liabilities, actions, judgments, costs and expenses (including reasonable attorneys’ fees) brought by a third party arising out of or in connection with: (a) any intentionally tortious or negligent act or omission by You in connection with Your use of the Licensed Material; (b) Your use of the Licensed Material other than as expressly allowed by this Agreement; (c) any claims that Derivatives or Simulator Content have infringed third party intellectual property rights; or (d) Your breach or alleged breach of any of the terms, restrictions, obligations or representations under this Agreement. -8 MISCELLANEOUS. +8 MISCELLANEOUS. This Agreement constitutes the entire agreement among the parties with respect to the subject matter and supersedes and merges all prior proposals, understandings and contemporaneous communications. Any modification to this Agreement must be in a writing duly authorized by LG or pursuant to Section 2 (Updates). You may not assign any of the rights or obligations granted hereunder, voluntarily or by operation of law (including without limitation in connection with a merger, acquisition, or sale of assets) except with the express written consent of LG, and any attempted assignment in violation of this paragraph is void. This Agreement does not create or imply any partnership, agency or joint venture. This Agreement will be governed by and construed in accordance with the laws of the State of California, without regard to or application of conflicts of law rules or principles. Any controversy arising out of this Agreement shall be heard exclusively in the federal or state courts located in Santa Clara County, CA and You agree to submit to jurisdiction thereof. No waiver by LG of any covenant or right under this Agreement will be effective unless memorialized in a writing duly authorized by LG. If any part of this Agreement is determined to be invalid or unenforceable by a court of competent jurisdiction, that provision will be enforced to the maximum extent permissible and the remaining provisions of this Agreement will remain in full force and effect. - + EXHIBIT A Licensed Material: diff --git a/README.md b/README.md index 5acd0c9..a1421d9 100644 --- a/README.md +++ b/README.md @@ -1,38 +1,107 @@ -# Python API for LGSVL Simulator +# lgsvl - A Python API for SVL Simulator -This folder contains Python API for LGSVL Simulator. - -# Usage - -Look into `quickstart` folder for simple usages. -To run these examples first start the simulator and leave it in main menu. -By default examples connect to Simulator on `localhost` address. -To change it, adjust first argument of `Simulator` constructor, or set up -`SIMULATOR_HOST` environment variable with hostname. +This folder contains **lgsvl**, a Python API for SVL Simulator. # Documentation -Documentation is available on our website: https://www.lgsvlsimulator.com/docs/python-api/ +Documentation is available on our website: https://www.svlsimulator.com/docs/python-api/ # Requirements -* Python 3.5 or higher +* Python 3.6 or higher +* Pip 21.1.1 or higher (install using `python3 -m pip install 'pip>=21.1.1'`) # Installing - pip3 install --user . - + python3 -m pip install -r requirements.txt --user . + # install in development mode - pip3 install --user -e . + python3 -m pip install -r requirements.txt --user -e . + +Do not use the legacy `python3 setup.py install` nor `pip3 install`. + +**NOTE:** If you are using release 2020.06 of SVL Simulator, you must switch to +the `release-2020.06` branch of this repository prior to installing: + + git checkout -b release-2020.06 + +We encourage you to migrate to release 2021.1 of SVL Simulator soon as it is +feasible/practical to do so. + +# Examples + +Look into `quickstart` and `examples` folders for simple usages. To run these +examples, first make sure that these assets have been added to your Library: + +| Type | Name | URL | +| ------- | -------------- | --- | +| Map | BorregasAve | https://wise.svlsimulator.com/maps/profile/aae03d2a-b7ca-4a88-9e41-9035287a12cc | +| Map | CubeTown | https://wise.svlsimulator.com/maps/profile/06773677-1ce3-492f-9fe2-b3147e126e27 | +| Map | SanFrancisco | https://wise.svlsimulator.com/maps/profile/5d272540-f689-4355-83c7-03bf11b6865f | +| Map | SingleLaneRoad | https://wise.svlsimulator.com/maps/profile/a6e2d149-6a18-4b83-9029-4411d7b2e69a | +| Map | Straight1LanePedestrianCrosswalk | https://wise.svlsimulator.com/maps/profile/a3a818b5-c66b-488a-a780-979bd5692db1 | +| Map | Straight1LaneSame | https://wise.svlsimulator.com/maps/profile/1e2287cf-c590-4804-bcb1-18b2fd3752d1 | +| Map | Straight2LaneOpposing | https://wise.svlsimulator.com/maps/profile/671868be-44f9-44a1-913c-cb0f29d12634 | +| Map | Straight2LaneSame | https://wise.svlsimulator.com/maps/profile/b39d3ef9-21d7-409d-851b-4c90dad80a25 | +| Map | Straight2LaneSameCurbRightIntersection | https://wise.svlsimulator.com/maps/profile/378edc3f-8fce-4596-87dc-7d12fc2ad743 | +| Vehicle | Jaguar2015XE | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb | +| Vehicle | Lincoln2017MKZ | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e | + +
+and that the plugins required by these vehicle sensor configurations have been added to your Library: + +| Vehicle | Sensor Configuration | URL | +| ------------ | -------------------- | --- | +Jaguar2015XE | Apollo 5.0 | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/c06d4932-5928-4730-8a91-ba64ac5f1813 | +Jaguar2015XE | Autoware AI | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/05cbb194-d095-4a0e-ae66-ff56c331ca83 | +Lincoln2017MKZ | Apollo 5.0 | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/47b529db-0593-4908-b3e7-4b24a32a0f70 | +Lincoln2017MKZ | Apollo 5.0 (Full Analysis) | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/22656c7b-104b-4e6a-9c70-9955b6582220 | +Lincoln2017MKZ | Apollo 5.0 (modular testing) | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/5c7fb3b0-1fd4-4943-8347-f73a05749718 | +
+ +Then launch a simulation on a local cluster selecting the **API Only** template. + +By default, the examples expect to be able to connect to SVL Simulator using the +`localhost` address. To change it, set the `LGSVL__SIMULATOR_HOST` environment +variable to the hostname or IP address of the network interface of the machine +running SVL Simulator to which the examples should connect. + # Running unit tests +To run the unit tests, first make sure that these assets have been added to your Library: + +| Type | Name | URL | +| ------- | -------------- | --- | +| Map | BorregasAve | https://wise.svlsimulator.com/maps/profile/aae03d2a-b7ca-4a88-9e41-9035287a12cc | +| Map | CubeTown | https://wise.svlsimulator.com/maps/profile/06773677-1ce3-492f-9fe2-b3147e126e27 | +| Vehicle | Jaguar2015XE | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb | +| Vehicle | Lincoln2017MKZ | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e | + +
+and that the plugins required by these vehicle sensor configurations have been added to your Library: + +| Vehicle | Sensor Configuration | URL | +| ------------ | -------------------- | --- | +Jaguar2015XE | Apollo 5.0 | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/c06d4932-5928-4730-8a91-ba64ac5f1813 | +Jaguar2015XE | Autoware AI | https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/05cbb194-d095-4a0e-ae66-ff56c331ca83 | +Lincoln2017MKZ | Apollo 5.0 | https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/47b529db-0593-4908-b3e7-4b24a32a0f70 | + +
+ +Then launch an **API Only** simulation before running the unit tests. + +By default, the unit tests expect to be able to connect to SVL Simulator using +the `localhost` address. To change it, set the `LGSVL__SIMULATOR_HOST` +environment variable to the hostname or IP address of the network interface of +the machine running SVL Simulator to which the unit tests should connect. + # run all unittests python3 -m unittest discover -v -c - + # run single test module python3 -m unittest -v -c tests/test_XXXX.py - + # run individual test case python3 -m unittest -v tests.test_XXX.TestCaseXXX.test_XXX python3 -m unittest -v tests.test_Simulator.TestSimulator.test_unload_scene @@ -41,17 +110,25 @@ Documentation is available on our website: https://www.lgsvlsimulator.com/docs/p # (one time only) install coverage.py pip3 install --user coverage - + # run all tests with coverage ~/.local/bin/coverage run -m unittest discover - + # generate html report ~/.local/bin/coverage html --omit "~/.local/*","tests/*" - + # output is in htmlcov/index.html +# Tagging releases + +The final change for a new release must be to set the argument of the +`get_version('')` call in `setup.py` to the new version. +Then commit this change and tag it with the new version: + + git tag -a -m + # Copyright and License -Copyright (c) 2018-2019 LG Electronics, Inc. +Copyright (c) 2018-2021 LG Electronics, Inc. This software contains code licensed as described in LICENSE. diff --git a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py b/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py deleted file mode 100755 index f6e8963..0000000 --- a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See EOV_C_25_20.py for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 11.111 # (40 km/h, 25 mph) -MAX_POV_SPEED = 8.889 # (32 km/h, 20 mph) -INITIAL_HEADWAY = 100 # spec says >30m -SPEED_VARIANCE = 4 -TIME_LIMIT = 30 -TIME_DELAY = 3 - -print("EOV_S_25_20 - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -# A point close to the desired lane was found in Editor. This method returns the position and orientation of the closest lane to the point. -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -finalPOVWaypointPosition = lgsvl.Vector(0, 0, -125) - -POVState = lgsvl.AgentState() -POVState.transform.position = lgsvl.Vector(finalPOVWaypointPosition.x, finalPOVWaypointPosition.y, finalPOVWaypointPosition.z + 4.5 + INITIAL_HEADWAY) -POVState.transform.rotation = lgsvl.Vector(0, 180, 0) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -POVWaypoints = [] -POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, MAX_POV_SPEED)) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) - POV.follow(POVWaypoints) - - while True: - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - - sim.run(0.5) - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py b/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py deleted file mode 100755 index cfd8afb..0000000 --- a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See EOV_C_25_20.py for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 20 # (72 km/h, 45 mph) -MAX_POV_SPEED = 17.778 # (64 km/h, 40 mph) -INITIAL_HEADWAY = 150 # spec says >68m -SPEED_VARIANCE = 4 -TIME_LIMIT = 30 -TIME_DELAY = 4 - -print("EOV_S_45_40 - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -# A point close to the desired lane was found in Editor. This method returns the position and orientation of the closest lane to the point. -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -finalPOVWaypointPosition = lgsvl.Vector(0, 0, -125) - -POVState = lgsvl.AgentState() -POVState.transform.position = lgsvl.Vector(finalPOVWaypointPosition.x, finalPOVWaypointPosition.y, finalPOVWaypointPosition.z + 4.5 + INITIAL_HEADWAY) -POVState.transform.rotation = lgsvl.Vector(0, 180, 0) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -POVWaypoints = [] -POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, MAX_POV_SPEED)) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) - POV.follow(POVWaypoints) - - while True: - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - - sim.run(0.5) - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py b/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py deleted file mode 100755 index faec563..0000000 --- a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See EOV_C_25_20.py for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 29.167 # (105 km/h, 65 mph) -MAX_POV_SPEED = 26.667 # (96 km/h, 60 mph) -INITIAL_HEADWAY = 200 # spec says >105m -SPEED_VARIANCE = 4 -TIME_LIMIT = 30 -TIME_DELAY = 5 - -print("EOV_S_65_60 - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -# A point close to the desired lane was found in Editor. This method returns the position and orientation of the closest lane to the point. -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -finalPOVWaypointPosition = lgsvl.Vector(0, 0, -125) - -POVState = lgsvl.AgentState() -POVState.transform.position = lgsvl.Vector(finalPOVWaypointPosition.x, finalPOVWaypointPosition.y, finalPOVWaypointPosition.z + 4.5 + INITIAL_HEADWAY) -POVState.transform.rotation = lgsvl.Vector(0, 180, 0) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -POVWaypoints = [] -POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, MAX_POV_SPEED)) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) - POV.follow(POVWaypoints) - - while True: - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - - sim.run(0.5) - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/__init__.py b/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/__init__.py deleted file mode 100644 index ba0e7b1..0000000 --- a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -from .utils import TestException, separation \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/utils.py b/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/utils.py deleted file mode 100644 index ccc6669..0000000 --- a/examples/NHTSA-sample-tests/Encroaching-Oncoming-Vehicles/evaluator/utils.py +++ /dev/null @@ -1,17 +0,0 @@ -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -import lgsvl -import math - -class TestException(Exception): - pass - -def separation(V1, V2): - xdiff = V1.x - V2.x - ydiff = V1.y - V2.y - zdiff = V1.z - V2.z - return math.sqrt(xdiff * xdiff + ydiff * ydiff + zdiff * zdiff) \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_25_Slow.py b/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_25_Slow.py deleted file mode 100755 index 61995df..0000000 --- a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_25_Slow.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See VF_C_25_Slow for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 11.18 # (40 km/h, 25 mph) -SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value -MAX_POV_SPEED = 8.94 # (32 km/h, 20 mph) -MAX_POV_ROTATION = 5 #deg/s -TIME_LIMIT = 30 # seconds -TIME_DELAY = 3 -MAX_FOLLOWING_DISTANCE = 50 # Apollo 3.5 is very cautious - -print("VF_S_25_Slow - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -sim.set_time_of_day(12) -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) -egoX = ego.state.position.x -egoY = ego.state.position.y -egoZ = ego.state.position.z - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -POVState = lgsvl.AgentState() -POVState.transform = sim.map_point_on_lane(lgsvl.Vector(egoX, egoY, egoZ + 30)) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) # The EGO should start moving first - POV.follow_closest_lane(True, MAX_POV_SPEED, False) - - while True: - sim.run(0.5) - - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: - raise evaluator.TestException("POV angular rotation exceeded limit, {} > {} deg/s".format(POVCurrentState.angular_velocity, MAX_POV_ROTATION)) - - if evaluator.separation(POVCurrentState.position, lgsvl.Vector(1.8, 0, 125)) < 5: - break - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -separation = evaluator.separation(egoCurrentState.position, POVCurrentState.position) -if separation > MAX_FOLLOWING_DISTANCE: - print("FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE)) -else: - print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_45_Slow.py b/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_45_Slow.py deleted file mode 100755 index c6f171c..0000000 --- a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_45_Slow.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See VF_C_25_Slow for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 20.12 # (72 km/h, 45 mph) -SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value -MAX_POV_SPEED = 17.88 # (64 km/h, 40 mph) -MAX_POV_ROTATION = 5 #deg/s -TIME_LIMIT = 25 # seconds -TIME_DELAY = 7 -MAX_FOLLOWING_DISTANCE = 100 # Apollo 3.5 is very cautious - -print("VF_S_45_Slow - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) -egoX = ego.state.position.x -egoY = ego.state.position.y -egoZ = ego.state.position.z - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -POVState = lgsvl.AgentState() -POVState.transform = sim.map_point_on_lane(lgsvl.Vector(egoX, egoY, egoZ + 68)) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) # The EGO should start moving first - POV.follow_closest_lane(True, MAX_POV_SPEED, False) - - while True: - sim.run(0.5) - - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: - raise evaluator.TestException("POV angular rotation exceeded limit, {} > {} deg/s".format(POVCurrentState.angular_velocity, MAX_POV_ROTATION)) - - if evaluator.separation(POVCurrentState.position, lgsvl.Vector(1.8, 0, 125)) < 5: - break - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -separation = evaluator.separation(egoCurrentState.position, POVCurrentState.position) -if separation > MAX_FOLLOWING_DISTANCE: - print("FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE)) -else: - print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_65_Slow.py b/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_65_Slow.py deleted file mode 100755 index 5442225..0000000 --- a/examples/NHTSA-sample-tests/Vehicle-Following/VF_S_65_Slow.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -# See VF_C_25_Slow for a commented script - -import os -import lgsvl -import sys -import time -import evaluator - -MAX_EGO_SPEED = 29.06 # (105 km/h, 65 mph) -SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value -MAX_POV_SPEED = 26.82 # (96 km/h, 60 mph) -MAX_POV_ROTATION = 5 #deg/s -TIME_LIMIT = 45 # seconds -TIME_DELAY = 5 -MAX_FOLLOWING_DISTANCE = 100 # Apollo 3.5 is very cautious - -print("VF_S_65_Slow - ", end = '') - -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "SingleLaneRoad": - sim.reset() -else: - sim.load("SingleLaneRoad") - -# spawn EGO in the 2nd to right lane -egoState = lgsvl.AgentState() -egoState.transform = sim.get_spawn()[0] -ego = sim.add_agent("Jaguar2015XE (Apollo 5.0)", lgsvl.AgentType.EGO, egoState) -egoX = ego.state.position.x -egoY = ego.state.position.y -egoZ = ego.state.position.z - -ego.connect_bridge(os.environ.get("BRIDGE_HOST", "127.0.0.1"), 9090) - -POVState = lgsvl.AgentState() -POVState.transform = sim.map_point_on_lane(lgsvl.Vector(egoX - 104.74, egoY, egoZ - 7.34)) -POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) - -def on_collision(agent1, agent2, contact): - raise evaluator.TestException("Ego collided with {}".format(agent2)) - -ego.on_collision(on_collision) -POV.on_collision(on_collision) - -try: - t0 = time.time() - sim.run(TIME_DELAY) # The EGO should start moving first - POV.follow_closest_lane(True, MAX_POV_SPEED, False) - - while True: - sim.run(0.5) - - egoCurrentState = ego.state - if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE)) - - POVCurrentState = POV.state - if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: - raise evaluator.TestException("POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE)) - if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: - raise evaluator.TestException("POV angular rotation exceeded limit, {} > {} deg/s".format(POVCurrentState.angular_velocity, MAX_POV_ROTATION)) - - if evaluator.separation(POVCurrentState.position, lgsvl.Vector(1.8, 0, 125)) < 5: - break - - if time.time() - t0 > TIME_LIMIT: - break -except evaluator.TestException as e: - print("FAILED: " + repr(e)) - exit() - -separation = evaluator.separation(egoCurrentState.position, POVCurrentState.position) -if separation > MAX_FOLLOWING_DISTANCE: - print("FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE)) -else: - print("PASSED") \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/__init__.py b/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/__init__.py deleted file mode 100644 index ba0e7b1..0000000 --- a/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -from .utils import TestException, separation \ No newline at end of file diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/utils.py b/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/utils.py deleted file mode 100644 index ccc6669..0000000 --- a/examples/NHTSA-sample-tests/Vehicle-Following/evaluator/utils.py +++ /dev/null @@ -1,17 +0,0 @@ -# -# Copyright (c) 2019 LG Electronics, Inc. -# -# This software contains code licensed as described in LICENSE. -# - -import lgsvl -import math - -class TestException(Exception): - pass - -def separation(V1, V2): - xdiff = V1.x - V2.x - ydiff = V1.y - V2.y - zdiff = V1.z - V2.z - return math.sqrt(xdiff * xdiff + ydiff * ydiff + zdiff * zdiff) \ No newline at end of file diff --git a/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py new file mode 100755 index 0000000..833c743 --- /dev/null +++ b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_25_20.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See EOV_C_25_20.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 11.111 # (40 km/h, 25 mph) +MAX_POV_SPEED = 8.889 # (32 km/h, 20 mph) +INITIAL_HEADWAY = 130 # spec says >30m +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 +TIME_DELAY = 3 + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("EOV_S_25_20 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2laneopposing) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(-1.6, 0, -65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneOpposing')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) + +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +finalPOVWaypointPosition = egoState.position - 2.15 * right + +POVState = lgsvl.AgentState() +POVState.transform.position = egoState.position + (4.5 + INITIAL_HEADWAY) * forward - 2.15 * right +POVState.transform.rotation = lgsvl.Vector(0, -180, 0) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + +POVWaypoints = [] +POVWaypoints.append(lgsvl.DriveWaypoint(POVState.transform.position, MAX_POV_SPEED, POVState.transform.rotation)) +POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, 0, POVState.transform.rotation)) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +try: + t0 = time.time() + sim.run(TIME_DELAY) + POV.follow(POVWaypoints) + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +print("PASSED") diff --git a/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py new file mode 100755 index 0000000..54fe9cc --- /dev/null +++ b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_45_40.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See EOV_C_25_20.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 20 # (72 km/h, 45 mph) +MAX_POV_SPEED = 17.778 # (64 km/h, 40 mph) +INITIAL_HEADWAY = 130 # spec says >68m +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 +TIME_DELAY = 4 + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("EOV_S_45_40 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2laneopposing) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(-1.6, 0, -65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneOpposing')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) + +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +finalPOVWaypointPosition = egoState.position - 2.15 * right + +POVState = lgsvl.AgentState() +POVState.transform.position = egoState.position + (4.5 + INITIAL_HEADWAY) * forward - 2.15 * right +POVState.transform.rotation = lgsvl.Vector(0, -180, 0) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + +POVWaypoints = [] +POVWaypoints.append(lgsvl.DriveWaypoint(POVState.transform.position, MAX_POV_SPEED, POVState.transform.rotation)) +POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, 0, POVState.transform.rotation)) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +try: + t0 = time.time() + sim.run(TIME_DELAY) + POV.follow(POVWaypoints) + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +print("PASSED") diff --git a/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py new file mode 100755 index 0000000..d53a662 --- /dev/null +++ b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_S_65_60.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See EOV_C_25_20.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 29.167 # (105 km/h, 65 mph) +MAX_POV_SPEED = 26.667 # (96 km/h, 60 mph) +INITIAL_HEADWAY = 130 # spec says >105m +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 +TIME_DELAY = 5 + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("EOV_S_65_60 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2laneopposing) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(-1.6, 0, -65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneOpposing')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +finalPOVWaypointPosition = egoState.position - 2.15 * right + +POVState = lgsvl.AgentState() +POVState.transform.position = egoState.position + (4.5 + INITIAL_HEADWAY) * forward - 2.15 * right +POVState.transform.rotation = lgsvl.Vector(0, -180, 0) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + +POVWaypoints = [] +POVWaypoints.append(lgsvl.DriveWaypoint(POVState.transform.position, MAX_POV_SPEED, POVState.transform.rotation)) +POVWaypoints.append(lgsvl.DriveWaypoint(finalPOVWaypointPosition, 0, POVState.transform.rotation)) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +try: + t0 = time.time() + sim.run(TIME_DELAY) + POV.follow(POVWaypoints) + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +print("PASSED") diff --git a/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_test_runner.sh b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_test_runner.sh new file mode 100755 index 0000000..fd96036 --- /dev/null +++ b/examples/NHTSA/Encroaching-Oncoming-Vehicles/EOV_test_runner.sh @@ -0,0 +1,7 @@ +#!/bin/sh + +set -eu + +python3 EOV_S_25_20.py +python3 EOV_S_45_40.py +python3 EOV_S_65_60.py \ No newline at end of file diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_15.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_15.py new file mode 100755 index 0000000..807ff17 --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_15.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See MOTL_Simp_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 24 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Comp_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Destination in the middle of the parking zone +destination = egoState.position + (2.75 + 4.6 + 11 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) + +POVList = [] + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + (4.55 + 11) * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.on_collision(on_collision) +POVList.append(POV1) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(POV1State.position + (PARKING_ZONE_LENGTH + 4.6) * forward) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.on_collision(on_collision) +POVList.append(POV2) + +for i in range(2,5): + POVState = lgsvl.AgentState() + POVState.transform = sim.map_point_on_lane(POVList[i-1].state.position + 5.5 * forward) + POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + POV.on_collision(on_collision) + POVList.append(POV) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + for i in range(len(POVList)): + POVCurrentState = POVList[i].state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV{} speed exceeded limit, {} > {} m/s".format(i, POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 11) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_25.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_25.py new file mode 100755 index 0000000..eb0a96a --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Comp_25.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See MOTL_Simp_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 24 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Comp_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Destination in the middle of the parking zone +destination = egoState.position + (2.75 + 4.6 + 11 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) + +POVList = [] + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + (4.55 + 11) * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.on_collision(on_collision) +POVList.append(POV1) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(POV1State.position + (PARKING_ZONE_LENGTH + 4.6) * forward) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.on_collision(on_collision) +POVList.append(POV2) + +for i in range(2,5): + POVState = lgsvl.AgentState() + POVState.transform = sim.map_point_on_lane(POVList[i-1].state.position + 5.5 * forward) + POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + POV.on_collision(on_collision) + POVList.append(POV) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + for i in range(len(POVList)): + POVCurrentState = POVList[i].state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV{} speed exceeded limit, {} > {} m/s".format(i, POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 11) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_15.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_15.py new file mode 100755 index 0000000..f79648c --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_15.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See MOTL_Simp_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 3 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Neg_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Destination in the middle of the parking zone +destination = egoState.position + (2.75 + 4.6 + 6 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) + +POVList = [] + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + (4.55 + 6) * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.on_collision(on_collision) +POVList.append(POV1) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(POV1State.position + (PARKING_ZONE_LENGTH + 4.6) * forward) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.on_collision(on_collision) +POVList.append(POV2) + +for i in range(2,11): + POVState = lgsvl.AgentState() + POVState.transform = sim.map_point_on_lane(POVList[i-1].state.position + 5.5 * forward) + POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + POV.on_collision(on_collision) + POVList.append(POV) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + for i in range(len(POVList)): + POVCurrentState = POVList[i].state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV{} speed exceeded limit, {} > {} m/s".format(i, POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 6) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_25.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_25.py new file mode 100755 index 0000000..d8fdc10 --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Neg_25.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See MOTL_Simp_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 3 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Neg_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Destination in the middle of the parking zone +destination = egoState.position + (2.75 + 4.6 + 6 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) + +POVList = [] + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + (4.55 + 6) * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.on_collision(on_collision) +POVList.append(POV1) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(POV1State.position + (PARKING_ZONE_LENGTH + 4.6) * forward) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.on_collision(on_collision) +POVList.append(POV2) + +for i in range(2,11): + POVState = lgsvl.AgentState() + POVState.transform = sim.map_point_on_lane(POVList[i-1].state.position + 5.5 * forward) + POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + POV.on_collision(on_collision) + POVList.append(POV) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + for i in range(len(POVList)): + POVCurrentState = POVList[i].state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV{} speed exceeded limit, {} > {} m/s".format(i, POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 6) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_15.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_15.py new file mode 100755 index 0000000..5e66f95 --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_15.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# The speed limit of the scenario may require the HD map or Apollo's planning configuration to be editted which is out of the scope of this script. + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST environment variables need to be set. The default for both is 127.0.0.1 (localhost). +# The scenario assumes that the EGO's destination is ahead in the right lane. + +# POV = Principal Other Vehicle (NPC) + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15 mph) +SPEED_VARIANCE = 4 # Without real physics, the calculation for a rigidbody's velocity is very imprecise +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 24 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Simp_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Ego position is the center of the model. +# Destination is half EGO length + NPC length + EGO-NPC bumper distance + half parking zone length ahead of EGO +destination = egoState.position + (2.75 + 4.6 + 12 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +# The POV is created in the right lane 12 m ahead of the EGO +POVState = lgsvl.AgentState() +POVState.transform = sim.map_point_on_lane(egoState.position + (4.55 + 12) * forward + 3.6 * right) +# The EGO is 4.5m long and the Sedan is 4.6m long. 4.55 is half the length of an EGO and Sedan +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +try: + t0 = time.time() + while True: + # The EGO should stay at or below the speed limit + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + # The POV should not move + POVCurrentState = POV.state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + # The above checks are made every 0.5 seconds + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +# These 2 positions are the beginning and end of the parking zone +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 12) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_25.py b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_25.py new file mode 100755 index 0000000..c3ce79b --- /dev/null +++ b/examples/NHTSA/Move-Out-of-Travel-Lane/MOTL_Simp_25.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See MOTL_Simp_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 30 # seconds +PARKING_ZONE_LENGTH = 24 # meters + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("MOTL_Simp_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesamecurbrightintersection) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(2.6, 0, 30)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSameCurbRightIntersection')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# Destination in the middle of the parking zone +destination = egoState.position + (2.75 + 4.6 + 12 + PARKING_ZONE_LENGTH / 2) * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POVState = lgsvl.AgentState() +POVState.transform = sim.map_point_on_lane(egoState.position + (4.55 + 12) * forward + 3.6 * right) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > 0 + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, 0 + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +parkingZoneBeginning = sim.map_point_on_lane(egoState.position + (2.75 + 4.6 + 12) * forward + 3.6 * right) +parkingZoneEnd = sim.map_point_on_lane(parkingZoneBeginning.position + PARKING_ZONE_LENGTH * forward) + +finalEgoState = ego.state + +try: + if not lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + raise lgsvl.evaluator.TestException("Ego did not change lanes") + elif not lgsvl.evaluator.in_parking_zone( + parkingZoneBeginning.position, + parkingZoneEnd.position, + finalEgoState.transform + ): + raise lgsvl.evaluator.TestException("Ego did not stop in parking zone") + elif finalEgoState.speed > 0.2: + raise lgsvl.evaluator.TestException("Ego did not park") + else: + print("PASSED") +except ValueError as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_B_15.py b/examples/NHTSA/Perform-Lane-Change/PLC_B_15.py new file mode 100755 index 0000000..8802dba --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_B_15.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_B_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_B_25.py b/examples/NHTSA/Perform-Lane-Change/PLC_B_25.py new file mode 100755 index 0000000..ec48ef9 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_B_25.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_B_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_B_35.py b/examples/NHTSA/Perform-Lane-Change/PLC_B_35.py new file mode 100755 index 0000000..d89dc52 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_B_35.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 15.556 # (56 km/h, 35mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_B_35 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_CP_15.py b/examples/NHTSA/Perform-Lane-Change/PLC_CP_15.py new file mode 100755 index 0000000..ff7d087 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_CP_15.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15mph) +SPEED_VARIANCE = 4 +MAX_POV_SEPARATION = 27 +SEPARATION_VARIANCE = 2 +TIME_LIMIT = 30 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_CP_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 13 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position - 13 * forward + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed ,MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format( + POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE + ) + ) + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_CP_25.py b/examples/NHTSA/Perform-Lane-Change/PLC_CP_25.py new file mode 100755 index 0000000..c8f3b75 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_CP_25.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +MAX_POV_SEPARATION = 27 +SEPARATION_VARIANCE = 2 +TIME_LIMIT = 30 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_CP_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 13 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position - 13 * forward + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed ,MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format( + POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE + ) + ) + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_CP_35.py b/examples/NHTSA/Perform-Lane-Change/PLC_CP_35.py new file mode 100755 index 0000000..cc469bf --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_CP_35.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 15.556 # (56 km/h, 35 mph) +SPEED_VARIANCE = 4 +MAX_POV_SEPARATION = 27 +SEPARATION_VARIANCE = 2 +TIME_LIMIT = 30 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_CP_35 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 13 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position - 13 * forward + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed ,MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format( + POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE + ) + ) + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SN_15.py b/examples/NHTSA/Perform-Lane-Change/PLC_SN_15.py new file mode 100755 index 0000000..c1f6fcb --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SN_15.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# This scenario simulates a situation where the EGO needs to change lanes in the presence of other traffic +# The speed limit of the scenario may require the HD map or Apollo's planning configuration to be editted which is out of the scope of this script. + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST environment variables need to be set. The default for both is 127.0.0.1 (localhost). +# The scenario assumes that the EGO's destination is ahead in the right lane. + +# POV = Principal Other Vehicle (NPC) + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15 mph) Max speed of EGO and POVs +SPEED_VARIANCE = 4 # Without real physics, the calculation for a rigidbody's velocity is very imprecise +MAX_POV_SEPARATION = 13 # The POVs in the right lane should keep a constant distance between them +SEPARATION_VARIANCE = 2 # The allowable variance in the POV separation +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SN_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) # Create the websocket connection to Dreamview +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) # Set the HD map in Dreamview +# Set the Vehicle configuration in Dreamview +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) + +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] # The list of modules to be enabled + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +# This is an lgsvl.Vector with the coordinates of the destination in the Unity coordinate system +destination = egoState.position + 120 * forward + 3.6 * right +# Will enable all the modules and then wait for Apollo to be ready before continuing +dv.setup_apollo(destination.x, destination.z, modules) + +# The first POV is positioned such that its rear bumper is 5m in front of the EGO's front bumper in the right lane +POV1State = lgsvl.AgentState() +# 5m ahead of EGO, +5m because the transforms are the centers of the models +POV1State.transform = sim.map_point_on_lane(egoState.position + 10 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +# The second POV is positioned such that its front bumper is even with the EGO's front bumper in the right lane +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + +# The third POV is positioned such that its front bumper is 8m behind the EGO's rear bumper in the same lane as the EGO +POV3State = lgsvl.AgentState() +POV3State.transform = sim.map_point_on_lane(egoState.position - 13 * forward + 3.6 * right) +POV3 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV3State) +POV3.follow_closest_lane(True, MAX_SPEED, False) + + +# Any collision results in a failed test +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) +POV3.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + # The EGO should not exceed the max specified speed + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POV3CurrentState = POV3.state + # The POVs in the right lane should maintain their starting separation + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format(POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE) + ) + # The POVs should not exceed the speed limit + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV3CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException("POV3 speed exceeded limit, {} > {} m/s".format(POV3CurrentState.speed, MAX_SPEED + SPEED_VARIANCE)) + # Stops the test when the first POV reaches the end of the road + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + # The above checks are made every 0.5 seconds. + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +# This checks that the EGO actually changed lanes +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SN_25.py b/examples/NHTSA/Perform-Lane-Change/PLC_SN_25.py new file mode 100755 index 0000000..4c1e9e5 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SN_25.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +MAX_POV_SEPARATION = 17 +SEPARATION_VARIANCE = 2 +TIME_LIMIT = 30 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SN_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 11 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + +POV3State = lgsvl.AgentState() +POV3State.transform = sim.map_point_on_lane(egoState.position - 11 * forward + 3.6 * right) +POV3 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV3State) +POV3.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) +POV3.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POV3CurrentState = POV3.state + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format(POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE) + ) + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV3CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException("POV3 speed exceeded limit, {} > {} m/s".format(POV3CurrentState.speed, MAX_SPEED + SPEED_VARIANCE)) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SN_35.py b/examples/NHTSA/Perform-Lane-Change/PLC_SN_35.py new file mode 100755 index 0000000..184d1b1 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SN_35.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 15.556 # (56 km/h, 35mph) +SPEED_VARIANCE = 4 +MAX_POV_SEPARATION = 19 +SEPARATION_VARIANCE = 2 +TIME_LIMIT = 30 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SN_35 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 13 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) +POV1.follow_closest_lane(True, MAX_SPEED, False) + +POV2State = lgsvl.AgentState() +POV2State.transform = sim.map_point_on_lane(egoState.position + 3.6 * right) +POV2 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV2State) +POV2.follow_closest_lane(True, MAX_SPEED, False) + +POV3State = lgsvl.AgentState() +POV3State.transform = sim.map_point_on_lane(egoState.position - 13 * forward + 3.6 * right) +POV3 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV3State) +POV3.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) +POV2.on_collision(on_collision) +POV3.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + POV2CurrentState = POV2.state + POV3CurrentState = POV3.state + POVSeparation = lgsvl.evaluator.separation(POV1CurrentState.position, POV2CurrentState.position) + if POVSeparation > MAX_POV_SEPARATION + SEPARATION_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 and POV2 are too far apart: {} > {}".format(POVSeparation, MAX_POV_SEPARATION + SEPARATION_VARIANCE) + ) + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV2CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV2 speed exceeded limit, {} > {} m/s".format(POV2CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if POV3CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV3 speed exceeded limit, {} > {} m/s".format(POV3CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SP_15.py b/examples/NHTSA/Perform-Lane-Change/PLC_SP_15.py new file mode 100755 index 0000000..218d91a --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SP_15.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 6.667 # (24 km/h, 15 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SP_15 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 11 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) + +POV1.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SP_25.py b/examples/NHTSA/Perform-Lane-Change/PLC_SP_25.py new file mode 100755 index 0000000..40233c4 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SP_25.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 11.111 # (40 km/h, 25 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SP_25 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 5 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) + +POV1.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_SP_35.py b/examples/NHTSA/Perform-Lane-Change/PLC_SP_35.py new file mode 100755 index 0000000..faf8a39 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_SP_35.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See PLC_SN_15.py for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_SPEED = 15.556 # (56 km/h, 35 mph) +SPEED_VARIANCE = 4 +TIME_LIMIT = 15 # seconds + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("PLC_SP_35 - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight2lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +# A point close to the desired lane was found in Editor. +# This method returns the position and orientation of the closest lane to the point. +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(3.68, 0, -50)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight2LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 120 * forward + 3.6 * right +dv.setup_apollo(destination.x, destination.z, modules) + +POV1State = lgsvl.AgentState() +POV1State.transform = sim.map_point_on_lane(egoState.position + 5 * forward + 3.6 * right) +POV1 = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POV1State) + +POV1.follow_closest_lane(True, MAX_SPEED, False) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("{} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +POV1.on_collision(on_collision) + +endOfRoad = egoState.position + 120 * forward + 3.6 * right + +try: + t0 = time.time() + while True: + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + POV1CurrentState = POV1.state + if POV1CurrentState.speed > MAX_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV1 speed exceeded limit, {} > {} m/s".format(POV1CurrentState.speed, MAX_SPEED + SPEED_VARIANCE) + ) + if lgsvl.evaluator.separation(POV1CurrentState.position, endOfRoad) < 5: + break + sim.run(0.5) + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +finalEgoState = ego.state +try: + if lgsvl.evaluator.right_lane_check(sim, finalEgoState.transform): + print("PASSED") + else: + raise lgsvl.evaluator.TestException("Ego did not change lanes") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Perform-Lane-Change/PLC_test_runner.sh b/examples/NHTSA/Perform-Lane-Change/PLC_test_runner.sh new file mode 100755 index 0000000..fe677b3 --- /dev/null +++ b/examples/NHTSA/Perform-Lane-Change/PLC_test_runner.sh @@ -0,0 +1,16 @@ +#!/bin/sh + +set -eu + +python3 PLC_B_15.py +python3 PLC_B_25.py +python3 PLC_B_35.py +python3 PLC_CP_15.py +python3 PLC_CP_25.py +python3 PLC_CP_35.py +python3 PLC_SN_15.py +python3 PLC_SN_25.py +python3 PLC_SN_35.py +python3 PLC_SP_15.py +python3 PLC_SP_25.py +python3 PLC_SP_35.py \ No newline at end of file diff --git a/examples/NHTSA/Vehicle-Following/VF_S_25_Slow.py b/examples/NHTSA/Vehicle-Following/VF_S_25_Slow.py new file mode 100755 index 0000000..5affb8e --- /dev/null +++ b/examples/NHTSA/Vehicle-Following/VF_S_25_Slow.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See VF_C_25_Slow for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 11.18 # (40 km/h, 25 mph) +SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value +MAX_POV_SPEED = 8.94 # (32 km/h, 20 mph) +MAX_POV_ROTATION = 5 # deg/s +TIME_LIMIT = 30 # seconds +TIME_DELAY = 3 +MAX_FOLLOWING_DISTANCE = 50 # Apollo 3.5 is very cautious + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("VF_S_25_Slow - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +sim.set_time_of_day(12) +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(1, 0, 65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight1LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +POVState = lgsvl.AgentState() +POVState.transform = sim.map_point_on_lane(egoState.position + 35 * forward) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +endOfRoad = egoState.position + 135 * forward + +try: + t0 = time.time() + sim.run(TIME_DELAY) # The EGO should start moving first + POV.follow_closest_lane(True, MAX_POV_SPEED, False) + while True: + sim.run(0.5) + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: + raise lgsvl.evaluator.TestException( + "POV angular rotation exceeded limit, {} > {} deg/s".format( + POVCurrentState.angular_velocity, MAX_POV_ROTATION + ) + ) + if lgsvl.evaluator.separation(POVCurrentState.position, endOfRoad) < 5: + break + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +separation = lgsvl.evaluator.separation(egoCurrentState.position, POVCurrentState.position) +try: + if separation > MAX_FOLLOWING_DISTANCE: + raise lgsvl.evaluator.TestException( + "FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE) + ) + else: + print("PASSED") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Vehicle-Following/VF_S_45_Slow.py b/examples/NHTSA/Vehicle-Following/VF_S_45_Slow.py new file mode 100755 index 0000000..4eed8c9 --- /dev/null +++ b/examples/NHTSA/Vehicle-Following/VF_S_45_Slow.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See VF_C_25_Slow for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 20.12 # (72 km/h, 45 mph) +SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value +MAX_POV_SPEED = 17.88 # (64 km/h, 40 mph) +MAX_POV_ROTATION = 5 # deg/s +TIME_LIMIT = 30 # seconds +TIME_DELAY = 7 +MAX_FOLLOWING_DISTANCE = 100 # Apollo 3.5 is very cautious + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("VF_S_45_Slow - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(1, 0, 65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight1LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +POVState = lgsvl.AgentState() +POVState.transform = sim.map_point_on_lane(egoState.position + 68 * forward) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +endOfRoad = egoState.position + 135 * forward + +try: + t0 = time.time() + sim.run(TIME_DELAY) # The EGO should start moving first + POV.follow_closest_lane(True, MAX_POV_SPEED, False) + while True: + sim.run(0.5) + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: + raise lgsvl.evaluator.TestException( + "POV angular rotation exceeded limit, {} > {} deg/s".format( + POVCurrentState.angular_velocity, MAX_POV_ROTATION + ) + ) + if lgsvl.evaluator.separation(POVCurrentState.position, endOfRoad) < 5: + break + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +separation = lgsvl.evaluator.separation(egoCurrentState.position, POVCurrentState.position) +try: + if separation > MAX_FOLLOWING_DISTANCE: + raise lgsvl.evaluator.TestException( + "FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE) + ) + else: + print("PASSED") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA/Vehicle-Following/VF_S_65_Slow.py b/examples/NHTSA/Vehicle-Following/VF_S_65_Slow.py new file mode 100755 index 0000000..774a731 --- /dev/null +++ b/examples/NHTSA/Vehicle-Following/VF_S_65_Slow.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# See VF_C_25_Slow for a commented script + +import time +import logging +from environs import Env +import lgsvl + +FORMAT = "[%(levelname)6s] [%(name)s] %(message)s" +logging.basicConfig(level=logging.WARNING, format=FORMAT) +log = logging.getLogger(__name__) + +env = Env() + +MAX_EGO_SPEED = 29.06 # (105 km/h, 65 mph) +SPEED_VARIANCE = 10 # Simple Physics does not return an accurate value +MAX_POV_SPEED = 26.82 # (96 km/h, 60 mph) +MAX_POV_ROTATION = 5 # deg/s +TIME_LIMIT = 30 # seconds +TIME_DELAY = 5 +MAX_FOLLOWING_DISTANCE = 100 # Apollo 3.5 is very cautious + +LGSVL__SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +LGSVL__SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +LGSVL__AUTOPILOT_0_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +LGSVL__AUTOPILOT_0_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +print("VF_S_65_Slow - ", end='') + +sim = lgsvl.Simulator(LGSVL__SIMULATOR_HOST, LGSVL__SIMULATOR_PORT) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanesame) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO in the 2nd to right lane +egoState = lgsvl.AgentState() +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(1, 0, 65)) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +forward = lgsvl.utils.transform_to_forward(egoState.transform) +right = lgsvl.utils.transform_to_right(egoState.transform) + +ego.connect_bridge(LGSVL__AUTOPILOT_0_HOST, LGSVL__AUTOPILOT_0_PORT) + +dv = lgsvl.dreamview.Connection(sim, ego, LGSVL__AUTOPILOT_0_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", 'Straight1LaneSame')) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is empty, using default list: {0}".format(modules)) + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] +except Exception: + modules = [ + 'Recorder', + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control' + ] + log.warning("LGSVL__AUTOPILOT_0_VEHICLE_MODULES is not set, using default list: {0}".format(modules)) + +destination = destination = egoState.position + 135 * forward +dv.setup_apollo(destination.x, destination.z, modules) + +POVState = lgsvl.AgentState() +POVState.transform = sim.map_point_on_lane(egoState.position + 105 * forward) +POV = sim.add_agent("Sedan", lgsvl.AgentType.NPC, POVState) + + +def on_collision(agent1, agent2, contact): + raise lgsvl.evaluator.TestException("Ego collided with {}".format(agent2)) + + +ego.on_collision(on_collision) +POV.on_collision(on_collision) + +endOfRoad = egoState.position + 135 * forward + +try: + t0 = time.time() + sim.run(TIME_DELAY) # The EGO should start moving first + POV.follow_closest_lane(True, MAX_POV_SPEED, False) + while True: + sim.run(0.5) + egoCurrentState = ego.state + if egoCurrentState.speed > MAX_EGO_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "Ego speed exceeded limit, {} > {} m/s".format(egoCurrentState.speed, MAX_EGO_SPEED + SPEED_VARIANCE) + ) + POVCurrentState = POV.state + if POVCurrentState.speed > MAX_POV_SPEED + SPEED_VARIANCE: + raise lgsvl.evaluator.TestException( + "POV speed exceeded limit, {} > {} m/s".format(POVCurrentState.speed, MAX_POV_SPEED + SPEED_VARIANCE) + ) + if POVCurrentState.angular_velocity.y > MAX_POV_ROTATION: + raise lgsvl.evaluator.TestException( + "POV angular rotation exceeded limit, {} > {} deg/s".format( + POVCurrentState.angular_velocity, MAX_POV_ROTATION + ) + ) + if lgsvl.evaluator.separation(POVCurrentState.position, endOfRoad) < 5: + break + if time.time() - t0 > TIME_LIMIT: + break +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) + +separation = lgsvl.evaluator.separation(egoCurrentState.position, POVCurrentState.position) +try: + if separation > MAX_FOLLOWING_DISTANCE: + raise lgsvl.evaluator.TestException( + "FAILED: EGO following distance was not maintained, {} > {}".format(separation, MAX_FOLLOWING_DISTANCE) + ) + else: + print("PASSED") +except lgsvl.evaluator.TestException as e: + exit("FAILED: {}".format(e)) diff --git a/examples/NHTSA-sample-tests/Vehicle-Following/VF_test_runner.sh b/examples/NHTSA/Vehicle-Following/VF_test_runner.sh similarity index 54% rename from examples/NHTSA-sample-tests/Vehicle-Following/VF_test_runner.sh rename to examples/NHTSA/Vehicle-Following/VF_test_runner.sh index 18da927..339f1f8 100755 --- a/examples/NHTSA-sample-tests/Vehicle-Following/VF_test_runner.sh +++ b/examples/NHTSA/Vehicle-Following/VF_test_runner.sh @@ -5,6 +5,6 @@ set -eu python3 VF_S_25_Slow.py python3 VF_S_45_Slow.py python3 VF_S_65_Slow.py -python3 VF_C_25_Slow.py -python3 VF_C_45_Slow.py -python3 VF_C_65_Slow.py +# python3 VF_C_25_Slow.py +# python3 VF_C_45_Slow.py +# python3 VF_C_65_Slow.py diff --git a/examples/Nav2/single-robot-cut-in.py b/examples/Nav2/single-robot-cut-in.py new file mode 100755 index 0000000..5ab9719 --- /dev/null +++ b/examples/Nav2/single-robot-cut-in.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Cut In") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +egoState = lgsvl.AgentState() +egoState.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(egoState.transform) +forward = lgsvl.utils.transform_to_forward(egoState.transform) + +pedState = lgsvl.AgentState() +pedState.transform.position = egoState.transform.position +pedState.transform.position += 5 * right + 5 * forward +ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState) + + +def on_collision(agent1, agent2, contact): + raise Exception("Failed: {} collided with {}".format(agent1, agent2)) + + +ego.on_collision(on_collision) +ped.on_collision(on_collision) +waypoints = [] +waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=6)) +waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 5 * right, idle=15)) +waypoints.append(lgsvl.WalkWaypoint(pedState.transform.position - 10 * right, idle=0)) +ped.follow(waypoints) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-freemode.py b/examples/Nav2/single-robot-freemode.py new file mode 100755 index 0000000..843c0f2 --- /dev/null +++ b/examples/Nav2/single-robot-freemode.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Freemode") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +state = lgsvl.AgentState() +state.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-sudden-braking.py b/examples/Nav2/single-robot-sudden-braking.py new file mode 100755 index 0000000..3adda33 --- /dev/null +++ b/examples/Nav2/single-robot-sudden-braking.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Sudden Braking") + +TIME_DELAY = 5 +TIME_LIMIT = 70 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +egoState = lgsvl.AgentState() +egoState.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(egoState.transform) +forward = lgsvl.utils.transform_to_forward(egoState.transform) + +pedState = lgsvl.AgentState() +pedState.transform = egoState.transform +pedState.transform.position += 2 * forward +ped = sim.add_agent("Robin", lgsvl.AgentType.PEDESTRIAN, pedState) + + +def on_collision(agent1, agent2, contact): + raise Exception("Failed: {} collided with {}".format(agent1.name, agent2.name)) + + +ego.on_collision(on_collision) +ped.on_collision(on_collision) +waypoints = [] +waypoints.append(lgsvl.WalkWaypoint(pedState.position, idle=0, trigger_distance=1.5)) +waypoints.append(lgsvl.WalkWaypoint(pedState.position + 3 * forward, 0, speed=1)) +ped.follow(waypoints) + +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/Nav2/single-robot-traffic-cones.py b/examples/Nav2/single-robot-traffic-cones.py new file mode 100755 index 0000000..85f7a59 --- /dev/null +++ b/examples/Nav2/single-robot-traffic-cones.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl + +print("Navigation2 Example: Single Robot Traffic Cones") + +TIME_DELAY = 5 +TIME_LIMIT = 90 +NAV_DST = ((10.9, -0, 0), (0, 0, 0, 1.0)) + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host) +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host) +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +SCENE = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_lgseocho) +ROBOT = os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == SCENE: + sim.reset() +else: + sim.load(SCENE, seed=0) + +spawns = sim.get_spawn() +state = lgsvl.AgentState() +state.transform = spawns[0] + +ego = sim.add_agent(ROBOT, lgsvl.AgentType.EGO, state) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) +ego.set_initial_pose() +sim.run(TIME_DELAY) +is_reached = False + + +def on_destination_reached(agent): + global is_reached + is_reached = True + print(f"Robot reached destination") + sim.stop() + return + + +sim_dst = sim.map_from_nav(lgsvl.Vector(*NAV_DST[0]), lgsvl.Quaternion(*NAV_DST[1])) +ego.set_destination(sim_dst) +res = ego.on_destination_reached(on_destination_reached) + +right = lgsvl.utils.transform_to_right(state.transform) +forward = lgsvl.utils.transform_to_forward(state.transform) + +sign = 1 +for j in range(2, 11, 2): + sign *= -1 + for i in range(5): + objState = lgsvl.ObjectState() + objState.transform.position = state.transform.position + offset = sign * 0.5 * i * right + j * forward + objState.transform.position += offset + sim.controllable_add("TrafficCone", objState) + + +def on_collision(agent1, agent2, contact): + name1 = "traffic cone" if agent1 is None else agent1.name + name2 = "traffic cone" if agent2 is None else agent2.name + raise Exception("Failed: {} collided with {}".format(name1, name2)) + + +ego.on_collision(on_collision) +print(f"Running simulation for {TIME_LIMIT} seconds...") +sim.run(TIME_LIMIT) + +if is_reached: + print("PASSED") +else: + exit("FAILED: Ego did not reach the destination") diff --git a/examples/SampleTestCases/cut-in.py b/examples/SampleTestCases/cut-in.py new file mode 100755 index 0000000..9a9006c --- /dev/null +++ b/examples/SampleTestCases/cut-in.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST are environment variables +# They can be set for the terminal instance with `export LGSVL__SIMULATOR_HOST=###` +# which will use the set value for all future commands in that terminal. +# To set the variable for only a particular execution, run the script in this way +# `LGSVL__SIMULATOR_HOST=### LGSVL__AUTOPILOT_0_HOST=### python3 cut-in.py` + +# LGSVL__SIMULATOR_HOST is the IP of the computer running the simulator +# LGSVL__AUTOPILOT_0_HOST is the IP of the computer running the bridge (from the perspective of the Simulator) +# If the simulator and bridge are running on the same computer, then the default values will work. +# Otherwise the variables must be set for in order to connect to the simulator and the bridge to receive data. +# LGSVL__SIMULATOR_PORT and LGSVL__AUTOPILOT_0_HOST need to be set if non-default ports will be used + +import os +import lgsvl + +import time +import sys + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", 8181)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) + +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_sanfrancisco) + +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name, seed=0) + +# spawn EGO +egoState = lgsvl.AgentState() +# Spawn point found in Unity Editor +egoState.transform = sim.map_point_on_lane(lgsvl.Vector(773.29, 10.24, -10.79)) +ego = sim.add_agent(os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) + +right = lgsvl.utils.transform_to_right(egoState.transform) # Unit vector in the right direction of the EGO +forward = lgsvl.utils.transform_to_forward(egoState.transform) # Unit vector in the forward direction of the EGO + +# spawn NPC +npcState = lgsvl.AgentState() +npcState.transform = egoState.transform +npcState.transform.position = egoState.position - 3.6 * right # NPC is 3.6m to the left of the EGO +npc = sim.add_agent("Jeep", lgsvl.AgentType.NPC, npcState) + +# This function will be called if a collision occurs +def on_collision(agent1, agent2, contact): + raise Exception("{} collided with {}".format(agent1, agent2)) + +ego.on_collision(on_collision) +npc.on_collision(on_collision) + +controlReceived = False + +# This function will be called when the Simulator receives the first message on the topic defined in the CheckControlSensor configuration +def on_control_received(agent, kind, context): + global controlReceived + # There can be multiple custom callbacks defined, this checks for the appropriate kind + if kind == "checkControl": + # Stops the Simulator running, this will only interrupt the first sim.run(30) call + sim.stop() + controlReceived = True + +ego.on_custom(on_control_received) + +# Run Simulator for at most 30 seconds for the AD stack to to initialize +sim.run(30) + +# If a Control message was not received, then the AD stack is not ready and the scenario should not continue +if not controlReceived: + raise Exception("AD stack is not ready after 30 seconds") + sys.exit() + +# NPC will follow the HD map at a max speed of 15 m/s (33 mph) and will not change lanes automatically +# The speed limit of the road is 20m/s so the EGO should drive faster than the NPC +npc.follow_closest_lane(follow=True, max_speed=8, isLaneChange=False) + +# t0 is the time when the Simulation started +t0 = time.time() + +# This will keep track of if the NPC has already changed lanes +npcChangedLanes = False + +# Run Simulation for 4 seconds before checking cut-in or end conditions +sim.run(4) + +# The Simulation will pause every 0.5 seconds to check 2 conditions +while True: + sim.run(0.5) + + # If the NPC has not already changed lanes then the distance between the NPC and EGO is calculated + if not npcChangedLanes: + egoCurrentState = ego.state + npcCurrentState = npc.state + + separationDistance = (egoCurrentState.position - npcCurrentState.position).magnitude() + + # If the EGO and NPC are within 15m, then NPC will change lanes to the right (in front of the EGO) + if separationDistance <= 15: + npc.change_lane(False) + npcChangedLanes = True + + # Simulation will be limited to running for 30 seconds total + if time.time() - t0 > 26: + break diff --git a/examples/SampleTestCases/ped-crossing.py b/examples/SampleTestCases/ped-crossing.py new file mode 100755 index 0000000..06bef16 --- /dev/null +++ b/examples/SampleTestCases/ped-crossing.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST are environment variables +# They can be set for the terminal instance with `export LGSVL__SIMULATOR_HOST=###` +# which will use the set value for all future commands in that terminal. +# To set the variable for only a particular execution, run the script in this way +# `LGSVL__SIMULATOR_HOST=### LGSVL__AUTOPILOT_0_HOST=### python3 cut-in.py` + +# LGSVL__SIMULATOR_HOST is the IP of the computer running the simulator +# LGSVL__AUTOPILOT_0_HOST is the IP of the computer running the bridge (from the perspective of the Simulator) +# If the simulator and bridge are running on the same computer, then the default values will work. +# Otherwise the variables must be set for in order to connect to the simulator and the bridge to receive data. +# LGSVL__SIMULATOR_PORT and LGSVL__AUTOPILOT_0_HOST need to be set if non-default ports will be used + +import os +import lgsvl + +import sys + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", 8181)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) + +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_straight1lanepedestriancrosswalk) +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name) + +# spawn EGO +egoState = lgsvl.AgentState() +egoState.transform = sim.get_spawn()[0] +ego = sim.add_agent(os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) + +# spawn PED +pedState = lgsvl.AgentState() +pedState.transform.position = lgsvl.Vector(-7.67, 0.2, 6.299) +ped = sim.add_agent("Bob", lgsvl.AgentType.PEDESTRIAN, pedState) + +# This function will be called if a collision occurs +def on_collision(agent1, agent2, contact): + raise Exception("{} collided with {}".format(agent1, agent2)) + +ego.on_collision(on_collision) +ped.on_collision(on_collision) + +controlReceived = False + +# This function will be called when the Simulator receives the first message on the topic defined in the CheckControlSensor configuration +def on_control_received(agent, kind, context): + global controlReceived + # There can be multiple custom callbacks defined, this checks for the appropriate kind + if kind == "checkControl": + # Stops the Simulator running, this will only interrupt the first sim.run(30) call + sim.stop() + controlReceived = True + +ego.on_custom(on_control_received) + +# Run Simulator for at most 30 seconds for the AD stack to to initialize +sim.run(30) + +# If a Control message was not received, then the AD stack is not ready and the scenario should not continue +if not controlReceived: + raise Exception("AD stack is not ready") + sys.exit() + +# Create the list of waypoints for the pedestrian to follow +waypoints = [] +# Fist waypoint is the same position that the PED is spawned. The PED will wait here until the EGO is within 50m +waypoints.append(lgsvl.WalkWaypoint(position=pedState.position, idle=0, trigger_distance=50)) +# Second waypoint is across the crosswalk +waypoints.append(lgsvl.WalkWaypoint(position=lgsvl.Vector(8.88, 0.2, 6.299), idle=0)) + +# List of waypoints is given to the pedestrian +ped.follow(waypoints) + +# Simulation will run for 30 seconds +sim.run(30) diff --git a/examples/SampleTestCases/random-traffic-local.py b/examples/SampleTestCases/random-traffic-local.py new file mode 100755 index 0000000..27939fe --- /dev/null +++ b/examples/SampleTestCases/random-traffic-local.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2020 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from datetime import datetime +from environs import Env +import random +import lgsvl + + +''' +LGSVL__AUTOPILOT_0_HOST IP address of the computer running the bridge to connect to +LGSVL__AUTOPILOT_0_PORT Port that the bridge listens on for messages +LGSVL__AUTOPILOT_0_VEHICLE_CONFIG Vehicle configuration to be loaded in Dreamview (Capitalization and spacing must match the dropdown in Dreamview) +LGSVL__AUTOPILOT_HD_MAP HD map to be loaded in Dreamview (Capitalization and spacing must match the dropdown in Dreamview) +LGSVL__MAP ID of map to be loaded in Simulator +LGSVL__RANDOM_SEED Simulation random seed +LGSVL__SIMULATION_DURATION_SECS How long to run the simulation for +LGSVL__SIMULATOR_HOST IP address of computer running simulator (Master node if a cluster) +LGSVL__SIMULATOR_PORT Port that the simulator allows websocket connections over +LGSVL__VEHICLE_0 ID of EGO vehicle to be loaded in Simulator +''' + +env = Env() + +SIMULATOR_HOST = env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1") +SIMULATOR_PORT = env.int("LGSVL__SIMULATOR_PORT", 8181) +BRIDGE_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +BRIDGE_PORT = env.int("LGSVL__AUTOPILOT_0_PORT", 9090) + +LGSVL__AUTOPILOT_HD_MAP = env.str("LGSVL__AUTOPILOT_HD_MAP", "SanFrancisco") +LGSVL__AUTOPILOT_0_VEHICLE_CONFIG = env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ') +LGSVL__SIMULATION_DURATION_SECS = 120.0 +LGSVL__RANDOM_SEED = env.int("LGSVL__RANDOM_SEED", 51472) + +vehicle_conf = env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_modular) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_sanfrancisco) +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +try: + print("Loading map {}...".format(scene_name)) + sim.load(scene_name, LGSVL__RANDOM_SEED) # laod map with random seed +except Exception: + if sim.current_scene == scene_name: + sim.reset() + else: + sim.load(scene_name) + + +# reset time of the day +sim.set_date_time(datetime(2020, 7, 1, 15, 0, 0, 0), True) + +spawns = sim.get_spawn() +# select spawn deterministically depending on the seed +spawn_index = LGSVL__RANDOM_SEED % len(spawns) + +state = lgsvl.AgentState() +state.transform = spawns[spawn_index] # TODO some sort of Env Variable so that user/wise can select from list +print("Loading vehicle {}...".format(vehicle_conf)) +ego = sim.add_agent(vehicle_conf, lgsvl.AgentType.EGO, state) + +print("Connecting to bridge...") +# The EGO is now looking for a bridge at the specified IP and port +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) + +def on_collision(agent1, agent2, contact): + raise Exception("{} collided with {}".format(agent1, agent2)) + sys.exit() + +ego.on_collision(on_collision) + +dv = lgsvl.dreamview.Connection(sim, ego, BRIDGE_HOST) +dv.set_hd_map(LGSVL__AUTOPILOT_HD_MAP) +dv.set_vehicle(LGSVL__AUTOPILOT_0_VEHICLE_CONFIG) + +destination_index = LGSVL__RANDOM_SEED % len(spawns[spawn_index].destinations) +destination = spawns[spawn_index].destinations[destination_index] # TODO some sort of Env Variable so that user/wise can select from list + +default_modules = [ + 'Localization', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Control', + 'Recorder' +] + +dv.disable_apollo() +dv.setup_apollo(destination.position.x, destination.position.z, default_modules) + +print("adding npcs") +sim.add_random_agents(lgsvl.AgentType.NPC) +sim.add_random_agents(lgsvl.AgentType.PEDESTRIAN) + +sim.run(LGSVL__SIMULATION_DURATION_SECS) diff --git a/examples/SampleTestCases/random-traffic.py b/examples/SampleTestCases/random-traffic.py new file mode 100755 index 0000000..7060531 --- /dev/null +++ b/examples/SampleTestCases/random-traffic.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from datetime import datetime +import random +from environs import Env + +import lgsvl + + +''' +LGSVL__AUTOPILOT_0_HOST IP address of the computer running the bridge to connect to +LGSVL__AUTOPILOT_0_PORT Port that the bridge listens on for messages +LGSVL__AUTOPILOT_0_VEHICLE_CONFIG Vehicle configuration to be loaded in Dreamview (Capitalization and spacing must match the dropdown in Dreamview) +LGSVL__AUTOPILOT_HD_MAP HD map to be loaded in Dreamview (Capitalization and spacing must match the dropdown in Dreamview) +LGSVL__AUTOPILOT_0_VEHICLE_MODULES List of modules to be enabled in Dreamview (Capitalization and space must match the sliders in Dreamview) +LGSVL__DATE_TIME Date and time to start simulation at, format 'YYYY-mm-ddTHH:MM:SS' +LGSVL__ENVIRONMENT_CLOUDINESS Value of clouds weather effect, clamped to [0, 1] +LGSVL__ENVIRONMENT_DAMAGE Value of road damage effect, clamped to [0, 1] +LGSVL__ENVIRONMENT_FOG Value of fog weather effect, clamped to [0, 1] +LGSVL__ENVIRONMENT_RAIN Value of rain weather effect, clamped to [0, 1] +LGSVL__ENVIRONMENT_WETNESS Value of wetness weather effect, clamped to [0, 1] +LGSVL__MAP Name of map to be loaded in Simulator +LGSVL__RANDOM_SEED Seed used to determine random factors (e.g. NPC type, color, behaviour) +LGSVL__SIMULATION_DURATION_SECS How long to run the simulation for +LGSVL__SIMULATOR_HOST IP address of computer running simulator (Master node if a cluster) +LGSVL__SIMULATOR_PORT Port that the simulator allows websocket connections over +LGSVL__SPAWN_BICYCLES Wether or not to spawn bicycles +LGSVL__SPAWN_PEDESTRIANS Whether or not to spawn pedestrians +LGSVL__SPAWN_TRAFFIC Whether or not to spawn NPC vehicles +LGSVL__TIME_OF_DAY If LGSVL__DATE_TIME is not set, today's date is used and this sets the time of day to start simulation at, clamped to [0, 24] +LGSVL__TIME_STATIC Whether or not time should remain static (True = time is static, False = time moves forward) +LGSVL__VEHICLE_0 Name of EGO vehicle to be loaded in Simulator +''' + +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", "127.0.0.1"), env.int("LGSVL__SIMULATOR_PORT", 8181)) +scene_name = env.str("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_borregasave) +try: + sim.load(scene_name, env.int("LGSVL__RANDOM_SEED")) +except Exception: + if sim.current_scene == scene_name: + sim.reset() + else: + sim.load(scene_name) + + +def clamp(val, min_v, max_v): + return min(max(val, min_v), max_v) + + +try: + date_time = env.str("LGSVL__DATE_TIME") +except Exception: + date_time = "1970-01-01T00:00:00" +else: + static_time = env.bool("LGSVL__TIME_STATIC", True) + if date_time == "1970-01-01T00:00:00": + time_of_day = clamp(env.float("LGSVL__TIME_OF_DAY", 12), 0, 24) + sim.set_time_of_day(time_of_day, static_time) + else: + sim.set_date_time(datetime.strptime(date_time[:19], '%Y-%m-%dT%H:%M:%S'), static_time) + + +rain = clamp(env.float("LGSVL__ENVIRONMENT_RAIN", 0), 0, 1) +fog = clamp(env.float("LGSVL__ENVIRONMENT_FOG", 0), 0, 1) +wetness = clamp(env.float("LGSVL__ENVIRONMENT_WETNESS", 0), 0, 1) +cloudiness = clamp(env.float("LGSVL__ENVIRONMENT_CLOUDINESS", 0), 0, 1) +damage = clamp(env.float("LGSVL__ENVIRONMENT_DAMAGE", 0), 0, 1) + +sim.weather = lgsvl.WeatherState(rain, fog, wetness, cloudiness, damage) + +spawns = sim.get_spawn() +spawn_index = random.randrange(len(spawns)) + +state = lgsvl.AgentState() +state.transform = spawns[spawn_index] # TODO some sort of Env Variable so that user/wise can select from list +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, state) + +# The EGO is now looking for a bridge at the specified IP and port +BRIDGE_HOST = env.str("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +ego.connect_bridge(BRIDGE_HOST, env.int("LGSVL__AUTOPILOT_0_PORT", 9090)) + +dv = lgsvl.dreamview.Connection(sim, ego, BRIDGE_HOST) +dv.set_hd_map(env.str("LGSVL__AUTOPILOT_HD_MAP", "Borregas Ave")) +dv.set_vehicle(env.str("LGSVL__AUTOPILOT_0_VEHICLE_CONFIG", 'Lincoln2017MKZ')) +default_modules = [ + 'Localization', + 'Perception', + 'Transform', + 'Routing', + 'Prediction', + 'Planning', + 'Traffic Light', + 'Control', + 'Recorder' +] + +try: + modules = env.list("LGSVL__AUTOPILOT_0_VEHICLE_MODULES", subcast=str) + if len(modules) == 0: + modules = default_modules +except Exception: + modules = default_modules + +destination_index = random.randrange(len(spawns[spawn_index].destinations)) +destination = spawns[spawn_index].destinations[destination_index] # TODO some sort of Env Variable so that user/wise can select from list +dv.setup_apollo(destination.position.x, destination.position.z, modules) + +if env.bool("LGSVL__SPAWN_TRAFFIC", True): + sim.add_random_agents(lgsvl.AgentType.NPC) + +if env.bool("LGSVL__SPAWN_PEDESTRIANS", True): + sim.add_random_agents(lgsvl.AgentType.PEDESTRIAN) + +sim.run(env.float("LGSVL__SIMULATION_DURATION_SECS", 0)) diff --git a/examples/SampleTestCases/red-light-runner.py b/examples/SampleTestCases/red-light-runner.py new file mode 100755 index 0000000..a974ba1 --- /dev/null +++ b/examples/SampleTestCases/red-light-runner.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST are environment variables +# They can be set for the terminal instance with `export LGSVL__SIMULATOR_HOST=###` +# which will use the set value for all future commands in that terminal. +# To set the variable for only a particular execution, run the script in this way +# `LGSVL__SIMULATOR_HOST=### LGSVL__AUTOPILOT_0_HOST=### python3 cut-in.py` + +# LGSVL__SIMULATOR_HOST is the IP of the computer running the simulator +# LGSVL__AUTOPILOT_0_HOST is the IP of the computer running the bridge (from the perspective of the Simulator) +# If the simulator and bridge are running on the same computer, then the default values will work. +# Otherwise the variables must be set for in order to connect to the simulator and the bridge to receive data. +# LGSVL__SIMULATOR_PORT and LGSVL__AUTOPILOT_0_HOST need to be set if non-default ports will be used + +import os +import lgsvl + +import sys + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", 8181)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) + +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_borregasave) +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name, seed=0) + +# spawn EGO +egoState = lgsvl.AgentState() +egoState.transform = sim.get_spawn()[0] +ego = sim.add_agent(os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) + +forward = lgsvl.utils.transform_to_forward(egoState.transform) # Unit vector in the forward direction of the EGO +right = lgsvl.utils.transform_to_right(egoState.transform) # Unit vector in the right direction of the EGO + +# spawn NPC +npcState = lgsvl.AgentState() +npcState.transform = sim.map_point_on_lane(egoState.position + 51.1 * forward + 29 * right) # NPC is 20m ahead of the EGO +npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, npcState) + +# This function will be called if a collision occurs +def on_collision(agent1, agent2, contact): + raise Exception("{} collided with {}".format(agent1, agent2)) + +ego.on_collision(on_collision) +npc.on_collision(on_collision) + +controlReceived = False + +# This function will be called when the Simulator receives the first message on the topic defined in the CheckControlSensor configuration +def on_control_received(agent, kind, context): + global controlReceived + # There can be multiple custom callbacks defined, this checks for the appropriate kind + if kind == "checkControl": + # Stops the Simulator running, this will only interrupt the first sim.run(30) call + sim.stop() + controlReceived = True + +ego.on_custom(on_control_received) + +# Run Simulator for at most 30 seconds for the AD stack to to initialize +sim.run(30) + +# If a Control message was not received, then the AD stack is not ready and the scenario should not continue +if not controlReceived: + raise Exception("AD stack is not ready") + sys.exit() + +# Create the list of waypoints for the npc to follow +waypoints = [] +# First waypoint is the same position that the npc is spawned. The npc will wait here until the EGO is within 50m +waypoints.append(lgsvl.DriveWaypoint(position=npcState.position, speed=20.1168, angle=npcState.rotation, idle=0, deactivate=False, trigger_distance=35)) +# Second waypoint is across the intersection +endPoint = sim.map_point_on_lane(egoState.position + 51.1 * forward - 63.4 * right) +waypoints.append(lgsvl.DriveWaypoint(position=endPoint.position, speed=0, angle=endPoint.rotation)) +npc.follow(waypoints) + +# Set all the traffic lights to green. +# It is possible to set only the lights visible by the EGO to green, but positions of the lights must be known +signals = sim.get_controllables("signal") +for signal in signals: + signal.control("green=3") + +# Run the simulation for 30 seconds +sim.run(30) diff --git a/examples/SampleTestCases/sudden-braking.py b/examples/SampleTestCases/sudden-braking.py new file mode 100755 index 0000000..802747d --- /dev/null +++ b/examples/SampleTestCases/sudden-braking.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +# LGSVL__SIMULATOR_HOST and LGSVL__AUTOPILOT_0_HOST are environment variables +# They can be set for the terminal instance with `export LGSVL__SIMULATOR_HOST=###` +# which will use the set value for all future commands in that terminal. +# To set the variable for only a particular execution, run the script in this way +# `LGSVL__SIMULATOR_HOST=### LGSVL__AUTOPILOT_0_HOST=### python3 cut-in.py` + +# LGSVL__SIMULATOR_HOST is the IP of the computer running the simulator +# LGSVL__AUTOPILOT_0_HOST is the IP of the computer running the bridge (from the perspective of the Simulator) +# If the simulator and bridge are running on the same computer, then the default values will work. +# Otherwise the variables must be set for in order to connect to the simulator and the bridge to receive data. +# LGSVL__SIMULATOR_PORT and LGSVL__AUTOPILOT_0_HOST need to be set if non-default ports will be used + +import os +import lgsvl + +import sys + +SIMULATOR_HOST = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") +SIMULATOR_PORT = int(os.environ.get("LGSVL__SIMULATOR_PORT", 8181)) +BRIDGE_HOST = os.environ.get("LGSVL__AUTOPILOT_0_HOST", "127.0.0.1") +BRIDGE_PORT = int(os.environ.get("LGSVL__AUTOPILOT_0_PORT", 9090)) + +scene_name = os.environ.get("LGSVL__MAP", lgsvl.wise.DefaultAssets.map_singlelaneroad) +sim = lgsvl.Simulator(SIMULATOR_HOST, SIMULATOR_PORT) +if sim.current_scene == scene_name: + sim.reset() +else: + sim.load(scene_name, seed=0) + +# spawn EGO +egoState = lgsvl.AgentState() +egoState.transform = sim.get_spawn()[0] +ego = sim.add_agent(os.environ.get("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5_full_analysis), lgsvl.AgentType.EGO, egoState) +ego.connect_bridge(BRIDGE_HOST, BRIDGE_PORT) + +forward = lgsvl.utils.transform_to_forward(egoState.transform) # Unit vector in the forward direction of the EGO + +# spawn NPC +npcState = lgsvl.AgentState() +npcState.transform = egoState.transform +npcState.transform.position = egoState.position + 20 * forward # NPC is 20m ahead of the EGO +npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, npcState) + +# This function will be called if a collision occurs +def on_collision(agent1, agent2, contact): + raise Exception("{} collided with {}".format(agent1, agent2)) + +ego.on_collision(on_collision) +npc.on_collision(on_collision) + +controlReceived = False + +# This function will be called when the Simulator receives the first message on the topic defined in the CheckControlSensor configuration +def on_control_received(agent, kind, context): + global controlReceived + # There can be multiple custom callbacks defined, this checks for the appropriate kind + if kind == "checkControl": + # Stops the Simulator running, this will only interrupt the first sim.run(30) call + sim.stop() + controlReceived = True + +ego.on_custom(on_control_received) + +# Run Simulator for at most 30 seconds for the AD stack to to initialize +sim.run(30) + +# If a Control message was not received, then the AD stack is not ready and the scenario should not continue +if not controlReceived: + raise Exception("AD stack is not ready") + sys.exit() + +# NPC will follow the HD map at a max speed of 11.176 m/s (25 mph) +npc.follow_closest_lane(follow=True, max_speed=11.176) + +# Run Simulation for 10 seconds +sim.run(10) + +# Force the NPC to come to a stop +control = lgsvl.NPCControl() +control.e_stop = True + +npc.apply_control(control) + +# Simulation will run for 10 seconds +# NPC will be stopped for this time +sim.run(10) + +# NPC told to resume following the HD map +npc.follow_closest_lane(True, 11.176) + +# Simulation runs for another 10 seconds +sim.run(10) diff --git a/examples/kitti_parser.py b/examples/kitti_parser.py index fc8eaca..cc00a43 100755 --- a/examples/kitti_parser.py +++ b/examples/kitti_parser.py @@ -11,7 +11,7 @@ # The data format is defined in a readme.txt downloadable from: https://s3.eu-central-1.amazonaws.com/avg-kitti/devkit_object.zip # Install numpy and PIL before running this script -# SIMULATOR_HOST environment variable also needs to be set before running the script +# LGSVL__SIMULATOR_HOST environment variable also needs to be set before running the script # 3 command line arguements are required when running this script. The arguements are: # number of data points to collect (int) @@ -70,7 +70,7 @@ def bootstrap(self): os.makedirs(LIDAR_BIN_PATH, exist_ok=True) os.makedirs(LABEL_PATH, exist_ok=True) - self.sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) + self.sim = lgsvl.Simulator(os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1"), 8181) self.load_scene() self.sim.reset() self.ego = self.sim.add_agent(self.agent_name, lgsvl.AgentType.EGO) @@ -115,7 +115,7 @@ def get_ego_random_transform(self): return transform -# Finds a random point near the EGO on the map. +# Finds a random point near the EGO on the map. # Once that is done, randomly find another nearby point so that the NPCs are spawned on different lanes. def get_npc_random_transform(self): ego_transform = self.ego_state.transform diff --git a/lgsvl/__init__.py b/lgsvl/__init__.py index 7e12606..be4e2ef 100644 --- a/lgsvl/__init__.py +++ b/lgsvl/__init__.py @@ -1,12 +1,30 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -from .geometry import Vector, BoundingBox, Transform +from .geometry import Vector, BoundingBox, Transform, Quaternion from .simulator import Simulator, RaycastHit, WeatherState from .sensor import Sensor, CameraSensor, LidarSensor, ImuSensor -from .agent import AgentType, AgentState, VehicleControl, Vehicle, EgoVehicle, NpcVehicle, Pedestrian, DriveWaypoint, WalkWaypoint, NPCControl +from .agent import ( + AgentType, + AgentState, + VehicleControl, + Vehicle, + EgoVehicle, + NpcVehicle, + Pedestrian, + DriveWaypoint, + WalkWaypoint, + WaypointTrigger, + TriggerEffector, + NPCControl, +) from .controllable import Controllable -from .utils import ObjectState \ No newline at end of file +from .utils import ObjectState + +# Subpackages +import lgsvl.dreamview +import lgsvl.evaluator +import lgsvl.wise diff --git a/lgsvl/agent.py b/lgsvl/agent.py index e60bb78..f9ca63e 100644 --- a/lgsvl/agent.py +++ b/lgsvl/agent.py @@ -1,299 +1,446 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -from .geometry import Vector, Transform, BoundingBox +from .geometry import Vector, BoundingBox, Transform from .sensor import Sensor from .utils import accepts, ObjectState as AgentState from enum import Enum from collections.abc import Iterable, Callable -import math +import json + class DriveWaypoint: - def __init__(self, position, speed, angle = Vector(0,0,0), idle = 0, deactivate = False, trigger_distance = 0, timestamp = -1): - self.position = position - self.speed = speed - self.angle = angle - self.idle = idle - self.deactivate = deactivate - self.trigger_distance = trigger_distance - self.timestamp = timestamp + def __init__( + self, + position, + speed, + acceleration=0, + angle=Vector(0, 0, 0), + idle=0, + deactivate=False, + trigger_distance=0, + timestamp=-1, + trigger=None, + ): + self.position = position + self.speed = speed + self.acceleration = acceleration + self.angle = angle + self.idle = idle + self.deactivate = deactivate + self.trigger_distance = trigger_distance + self.timestamp = timestamp + self.trigger = trigger + class WalkWaypoint: - def __init__(self, position, idle, trigger_distance = 0): - self.position = position - self.idle = idle - self.trigger_distance = trigger_distance + def __init__(self, position, idle, trigger_distance=0, speed=1, acceleration=0, trigger=None): + self.position = position + self.speed = speed + self.acceleration = acceleration + self.idle = idle + self.trigger_distance = trigger_distance + self.trigger = trigger + + +class WaypointTrigger: + def __init__(self, effectors): + self.effectors = effectors + + @staticmethod + def from_json(j): + return WaypointTrigger(json.loads(j["effectors"])) + + def to_json(self): + effectors_json = [] + for effector in self.effectors: + effectors_json.append(effector.to_json()) + return {"effectors": effectors_json} + + +class TriggerEffector: + def __init__(self, type_name, parameters): + self.type_name = type_name + self.parameters = parameters + + @staticmethod + def from_json(j): + return TriggerEffector(j["type_name"], j["parameters"]) + + def to_json(self): + return {"type_name": self.type_name, "parameters": self.parameters} + class AgentType(Enum): - EGO = 1 - NPC = 2 - PEDESTRIAN = 3 + EGO = 1 + NPC = 2 + PEDESTRIAN = 3 class VehicleControl: - def __init__(self): - self.steering = 0.0 # [-1..+1] - self.throttle = 0.0 # [0..1] - self.braking = 0.0 # [0..1] - self.reverse = False - self.handbrake = False - - # optional - self.headlights = None # int, 0=off, 1=low, 2=high beams - self.windshield_wipers = None # int, 0=off, 1-3=on - self.turn_signal_left = None # bool - self.turn_signal_right = None # bool + def __init__(self): + self.steering = 0.0 # [-1..+1] + self.throttle = 0.0 # [0..1] + self.braking = 0.0 # [0..1] + self.reverse = False + self.handbrake = False + + # optional + self.headlights = None # int, 0=off, 1=low, 2=high beams + self.windshield_wipers = None # int, 0=off, 1-3=on + self.turn_signal_left = None # bool + self.turn_signal_right = None # bool + class NPCControl: - def __init__(self): - self.headlights = None # int, 0=off, 1=low, 2=high - self.hazards = None # bool - self.e_stop = None # bool - self.turn_signal_left = None # bool - self.turn_signal_right = None # bool + def __init__(self): + self.headlights = None # int, 0=off, 1=low, 2=high + self.hazards = None # bool + self.e_stop = None # bool + self.turn_signal_left = None # bool + self.turn_signal_right = None # bool + class Agent: - def __init__(self, uid, simulator): - self.uid = uid - self.remote = simulator.remote - self.simulator = simulator - - @property - def state(self): - j = self.remote.command("agent/state/get", {"uid": self.uid}) - return AgentState.from_json(j) - - @state.setter - @accepts(AgentState) - def state(self, state): - self.remote.command("agent/state/set", { - "uid": self.uid, - "state": state.to_json() - }) - - @property - def transform(self): - return self.state.transform - - @property - def bounding_box(self): - j = self.remote.command("agent/bounding_box/get", {"uid": self.uid}) - return BoundingBox.from_json(j) - - def __eq__(self, other): - return self.uid == other.uid - - def __hash__(self): - return hash(self.uid) - - @accepts(Callable) - def on_collision(self, fn): - self.remote.command("agent/on_collision", {"uid": self.uid}) - self.simulator._add_callback(self, "collision", fn) - - @staticmethod - def create(simulator, uid, agent_type): - if agent_type == AgentType.EGO: - return EgoVehicle(uid, simulator) - elif agent_type == AgentType.NPC: - return NpcVehicle(uid, simulator) - elif agent_type == AgentType.PEDESTRIAN: - return Pedestrian(uid, simulator) - else: - raise ValueError("unsupported agent type") + def __init__(self, uid, simulator): + self.uid = uid + self.remote = simulator.remote + self.simulator = simulator + + @property + def state(self): + j = self.remote.command("agent/state/get", {"uid": self.uid}) + return AgentState.from_json(j) + + @state.setter + @accepts(AgentState) + def state(self, state): + self.remote.command( + "agent/state/set", {"uid": self.uid, "state": state.to_json()} + ) + + @property + def transform(self): + return self.state.transform + + @property + def bounding_box(self): + j = self.remote.command("agent/bounding_box/get", {"uid": self.uid}) + return BoundingBox.from_json(j) + + def __eq__(self, other): + return self.uid == other.uid + + def __hash__(self): + return hash(self.uid) + + @accepts(Callable) + def on_collision(self, fn): + self.remote.command("agent/on_collision", {"uid": self.uid}) + self.simulator._add_callback(self, "collision", fn) + + @staticmethod + def create(simulator, uid, agent_type): + if agent_type == AgentType.EGO: + return EgoVehicle(uid, simulator) + elif agent_type == AgentType.NPC: + return NpcVehicle(uid, simulator) + elif agent_type == AgentType.PEDESTRIAN: + return Pedestrian(uid, simulator) + else: + raise ValueError("unsupported agent type") class Vehicle(Agent): - def __init__(self, uid, simulator): - super().__init__(uid, simulator) + def __init__(self, uid, simulator): + super().__init__(uid, simulator) class EgoVehicle(Vehicle): - def __init__(self, uid, simulator): - super().__init__(uid, simulator) - - @property - def bridge_connected(self): - return self.remote.command("vehicle/bridge/connected", {"uid": self.uid}) - - @accepts(str, int) - def connect_bridge(self, address, port): - if port <= 0 or port > 65535: raise ValueError("port value is out of range") - self.remote.command("vehicle/bridge/connect", {"uid": self.uid, "address": address, "port": port}) - - def get_sensors(self): - j = self.remote.command("vehicle/sensors/get", {"uid": self.uid}) - return [Sensor.create(self.remote, sensor) for sensor in j] - - @accepts(bool, float) - def set_fixed_speed(self, isCruise, speed=None): - self.remote.command("vehicle/set_fixed_speed", {"uid": self.uid, "isCruise": isCruise, "speed": speed}) - - @accepts(VehicleControl, bool) - def apply_control(self, control, sticky = False): - args = { - "uid": self.uid, - "sticky": sticky, - "control": { - "steering": control.steering, - "throttle": control.throttle, - "braking": control.braking, - "reverse": control.reverse, - "handbrake": control.handbrake, - } - } - if control.headlights is not None: - args["control"]["headlights"] = control.headlights - if control.windshield_wipers is not None: - args["control"]["windshield_wipers"] = control.windshield_wipers - if control.turn_signal_left is not None: - args["control"]["turn_signal_left"] = control.turn_signal_left - if control.turn_signal_right is not None: - args["control"]["turn_signal_right"] = control.turn_signal_right - self.remote.command("vehicle/apply_control", args) - - def on_custom(self, fn): - self.simulator._add_callback(self, "custom", fn) + def __init__(self, uid, simulator): + super().__init__(uid, simulator) + + @property + def bridge_connected(self): + return self.remote.command("vehicle/bridge/connected", {"uid": self.uid}) + + @accepts(str, int) + def connect_bridge(self, address, port): + if port <= 0 or port > 65535: + raise ValueError("port value is out of range") + self.remote.command( + "vehicle/bridge/connect", + {"uid": self.uid, "address": address, "port": port}, + ) + + def get_bridge_type(self): + return self.remote.command("vehicle/bridge/type", {"uid": self.uid}) + + def get_sensors(self): + j = self.remote.command("vehicle/sensors/get", {"uid": self.uid}) + return [Sensor.create(self.remote, sensor) for sensor in j] + + @accepts(bool, float) + def set_fixed_speed(self, isCruise, speed=None): + self.remote.command( + "vehicle/set_fixed_speed", + {"uid": self.uid, "isCruise": isCruise, "speed": speed}, + ) + + @accepts(VehicleControl, bool) + def apply_control(self, control, sticky=False): + args = { + "uid": self.uid, + "sticky": sticky, + "control": { + "steering": control.steering, + "throttle": control.throttle, + "braking": control.braking, + "reverse": control.reverse, + "handbrake": control.handbrake, + }, + } + if control.headlights is not None: + args["control"]["headlights"] = control.headlights + if control.windshield_wipers is not None: + args["control"]["windshield_wipers"] = control.windshield_wipers + if control.turn_signal_left is not None: + args["control"]["turn_signal_left"] = control.turn_signal_left + if control.turn_signal_right is not None: + args["control"]["turn_signal_right"] = control.turn_signal_right + self.remote.command("vehicle/apply_control", args) + + def on_custom(self, fn): + self.simulator._add_callback(self, "custom", fn) + + def set_initial_pose(self): + self.remote.command( + "vehicle/set_initial_pose", + { + "uid": self.uid, + } + ) + + @accepts(Transform) + def set_destination(self, transform): + self.remote.command( + "vehicle/set_destination", + { + "uid": self.uid, + "transform": transform.to_json(), + } + ) + + def on_destination_reached(self, fn): + self.remote.command("agent/on_destination_reached", {"uid": self.uid}) + self.simulator._add_callback(self, "destination_reached", fn) class NpcVehicle(Vehicle): - def __init__(self, uid, simulator): - super().__init__(uid, simulator) - - @accepts(Iterable, bool) - def follow(self, waypoints, loop = False): - '''Tells the NPC to follow the waypoints - - When an NPC reaches a waypoint, it will: - 1. Wait for an EGO vehicle to approach to within the trigger_distance [meters] (ignored if 0) - 2. Wait the idle time (ignored if 0) - 3. Drive to the next waypoint (if any) - - Parameters - ---------- - waypoints : list of DriveWaypoints - DriveWaypoint : Class (position, speed, angle, idle, trigger_distance) - - position : lgsvl.Vector() - Unity coordinates of waypoint - - speed : float - how fast the NPC should drive to the waypoint - - angle : lgsvl.Vector() - Unity rotation of the NPC at the waypoint - - idle : float - time for the NPC to wait at the waypoint - - deactivate : bool - whether the NPC is to deactivate while waiting at this waypoint - - trigger_distance : float - how close an EGO must approach for the NPC to continue - - loop : bool - whether the NPC should loop through the waypoints after reaching the final one - ''' - self.remote.command("vehicle/follow_waypoints", { - "uid": self.uid, - "waypoints": [{ - "position": wp.position.to_json(), - "speed": wp.speed, - "angle": wp.angle.to_json(), - "idle": wp.idle, - "deactivate": wp.deactivate, - "trigger_distance": wp.trigger_distance, - "timestamp": wp.timestamp - } for wp in waypoints], - "loop": loop, - }) - - def follow_closest_lane(self, follow, max_speed, isLaneChange=True): - self.remote.command("vehicle/follow_closest_lane", {"uid": self.uid, "follow": follow, "max_speed": max_speed, "isLaneChange": isLaneChange}) - - @accepts(bool) - def change_lane(self, isLeftChange): - self.remote.command("vehicle/change_lane", {"uid": self.uid, "isLeftChange": isLeftChange}) - - @accepts(NPCControl) - def apply_control(self, control): - args = { - "uid": self.uid, - "control":{} - } - if control.headlights is not None: - if not control.headlights in [0,1,2]: - raise ValueError("unsupported intensity value") - args["control"]["headlights"] = control.headlights - if control.hazards is not None: - args["control"]["hazards"] = control.hazards - if control.e_stop is not None: - args["control"]["e_stop"] = control.e_stop - if control.turn_signal_left is not None or control.turn_signal_right is not None: - args["control"]["isLeftTurnSignal"] = control.turn_signal_left - args["control"]["isRightTurnSignal"] = control.turn_signal_right - self.remote.command("vehicle/apply_npc_control", args) - - def on_waypoint_reached(self, fn): - self.remote.command("agent/on_waypoint_reached", {"uid": self.uid}) - self.simulator._add_callback(self, "waypoint_reached", fn) - - def on_stop_line(self, fn): - self.remote.command("agent/on_stop_line", {"uid": self.uid}) - self.simulator._add_callback(self, "stop_line", fn) - - def on_lane_change(self, fn): - self.remote.command("agent/on_lane_change", {"uid": self.uid}) - self.simulator._add_callback(self, "lane_change", fn) + def __init__(self, uid, simulator): + super().__init__(uid, simulator) + + @accepts(Iterable, bool, str) + def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): + """Tells the NPC to follow the waypoints + + When an NPC reaches a waypoint, it will: + 1. Wait for an EGO vehicle to approach to within the trigger_distance [meters] (ignored if 0) + 2. Wait the idle time (ignored if 0) + 3. Drive to the next waypoint (if any) + + Parameters + ---------- + waypoints : list of DriveWaypoints + DriveWaypoint : Class (position, speed, acceleration, angle, idle, trigger_distance) + + position : lgsvl.Vector() + Unity coordinates of waypoint + + speed : float + how fast the NPC should drive to the waypoint + + acceleration : float + how fast the NPC will increase the speed + + angle : lgsvl.Vector() + Unity rotation of the NPC at the waypoint + + idle : float + time for the NPC to wait at the waypoint + + deactivate : bool + whether the NPC is to deactivate while waiting at this waypoint + + trigger_distance : float + how close an EGO must approach for the NPC to continue + + trigger : Class (list of Effectors) + trigger data with effectors applied on this waypoint + effectors : Class (type, value) + typeName : string + effector type name + parameters : dictionary + parameters of the effector (for example "value", "max_distance", "radius") + + loop : bool + whether the NPC should loop through the waypoints after reaching the final one + + waypoints_path_type : string + how the waypoints path should be interpreted, default path type is "Linear" + """ + self.remote.command( + "vehicle/follow_waypoints", + { + "uid": self.uid, + "waypoints": [ + { + "position": wp.position.to_json(), + "speed": wp.speed, + "acceleration": wp.acceleration, + "angle": wp.angle.to_json(), + "idle": wp.idle, + "deactivate": wp.deactivate, + "trigger_distance": wp.trigger_distance, + "timestamp": wp.timestamp, + "trigger": ( + None if wp.trigger is None else wp.trigger.to_json() + ), + } + for wp in waypoints + ], + "waypoints_path_type": waypoints_path_type, + "loop": loop, + }, + ) + + def follow_closest_lane(self, follow, max_speed, isLaneChange=True): + self.remote.command( + "vehicle/follow_closest_lane", + { + "uid": self.uid, + "follow": follow, + "max_speed": max_speed, + "isLaneChange": isLaneChange, + }, + ) + + def set_behaviour(self, behaviour): + self.remote.command( + "vehicle/behaviour", {"uid": self.uid, "behaviour": behaviour} + ) + + @accepts(bool) + def change_lane(self, isLeftChange): + self.remote.command( + "vehicle/change_lane", {"uid": self.uid, "isLeftChange": isLeftChange} + ) + + @accepts(NPCControl) + def apply_control(self, control): + args = {"uid": self.uid, "control": {}} + if control.headlights is not None: + if control.headlights not in [0, 1, 2]: + raise ValueError("unsupported intensity value") + args["control"]["headlights"] = control.headlights + if control.hazards is not None: + args["control"]["hazards"] = control.hazards + if control.e_stop is not None: + args["control"]["e_stop"] = control.e_stop + if ( + control.turn_signal_left is not None + or control.turn_signal_right is not None + ): + args["control"]["isLeftTurnSignal"] = control.turn_signal_left + args["control"]["isRightTurnSignal"] = control.turn_signal_right + self.remote.command("vehicle/apply_npc_control", args) + + def on_waypoint_reached(self, fn): + self.remote.command("agent/on_waypoint_reached", {"uid": self.uid}) + self.simulator._add_callback(self, "waypoint_reached", fn) + + def on_stop_line(self, fn): + self.remote.command("agent/on_stop_line", {"uid": self.uid}) + self.simulator._add_callback(self, "stop_line", fn) + + def on_lane_change(self, fn): + self.remote.command("agent/on_lane_change", {"uid": self.uid}) + self.simulator._add_callback(self, "lane_change", fn) class Pedestrian(Agent): - def __init__(self, uid, simulator): - super().__init__(uid, simulator) - - @accepts(bool) - def walk_randomly(self, enable): - self.remote.command("pedestrian/walk_randomly", {"uid": self.uid, "enable": enable}) - - @accepts(Iterable, bool) - def follow(self, waypoints, loop = False): - '''Tells the Pedestrian to follow the waypoints - - When a pedestrian reaches a waypoint, it will: - 1. Wait for an EGO vehicle to approach to within the trigger_distance [meters] (ignored if 0) - 2. Wait the idle time (ignored if 0) - 3. Walk to the next waypoint (if any) - - Parameters - ---------- - waypoints : list of WalkWaypoints - WalkWaypoint : Class (position, idle, trigger_distance) - - position : lgsvl.Vector() - Unity coordinates of waypoint - - idle : float - time for the pedestrian to wait at the waypoint - - trigger_distance : float - how close an EGO must approach for the pedestrian to continue - - loop : bool - whether the pedestrian should loop through the waypoints after reaching the final one - ''' - self.remote.command("pedestrian/follow_waypoints", { - "uid": self.uid, - "waypoints": [{"position": wp.position.to_json(), "idle": wp.idle, "trigger_distance": wp.trigger_distance} for wp in waypoints], - "loop": loop, - }) - - @accepts(Callable) - def on_waypoint_reached(self, fn): - self.remote.command("agent/on_waypoint_reached", {"uid": self.uid}) - self.simulator._add_callback(self, "waypoint_reached", fn) - \ No newline at end of file + def __init__(self, uid, simulator): + super().__init__(uid, simulator) + + @accepts(bool) + def walk_randomly(self, enable): + self.remote.command( + "pedestrian/walk_randomly", {"uid": self.uid, "enable": enable} + ) + + @accepts(Iterable, bool, str) + def follow(self, waypoints, loop=False, waypoints_path_type="Linear"): + """Tells the Pedestrian to follow the waypoints + + When a pedestrian reaches a waypoint, it will: + 1. Wait for an EGO vehicle to approach to within the trigger_distance [meters] (ignored if 0) + 2. Wait the idle time (ignored if 0) + 3. Walk to the next waypoint (if any) + + Parameters + ---------- + waypoints : list of WalkWaypoints + WalkWaypoint : Class (position, idle, trigger_distance, speed, acceleration) + + position : lgsvl.Vector() + Unity coordinates of waypoint + + idle : float + time for the pedestrian to wait at the waypoint + + trigger_distance : float + how close an EGO must approach for the pedestrian to continue + + speed : float + how fast the pedestrian should drive to the waypoint (default value 1) + + acceleration : float + how fast the pedestrian will increase the speed + + loop : bool + whether the pedestrian should loop through the waypoints after reaching the final one + """ + self.remote.command( + "pedestrian/follow_waypoints", + { + "uid": self.uid, + "waypoints": [ + { + "position": wp.position.to_json(), + "idle": wp.idle, + "trigger_distance": wp.trigger_distance, + "speed": wp.speed, + "acceleration": wp.acceleration, + "trigger": ( + None if wp.trigger is None else wp.trigger.to_json() + ), + } + for wp in waypoints + ], + "waypoints_path_type": waypoints_path_type, + "loop": loop, + }, + ) + + @accepts(float) + def set_speed(self, speed): + self.remote.command("pedestrian/set_speed", {"uid": self.uid, "speed": speed}) + + @accepts(Callable) + def on_waypoint_reached(self, fn): + self.remote.command("agent/on_waypoint_reached", {"uid": self.uid}) + self.simulator._add_callback(self, "waypoint_reached", fn) diff --git a/lgsvl/controllable.py b/lgsvl/controllable.py index 0b3e75d..16f116d 100644 --- a/lgsvl/controllable.py +++ b/lgsvl/controllable.py @@ -1,61 +1,62 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -from .geometry import Vector, Transform +from .geometry import Transform from .utils import accepts, ObjectState + class Controllable: - def __init__(self, remote, j): - self.remote = remote - self.uid = j["uid"] - self.type = j["type"] - self.transform = Transform.from_json(j) - self.valid_actions = j["valid_actions"] - self.default_control_policy = j["default_control_policy"] - - @property - def object_state(self): - j = self.remote.command("controllable/object_state/get", {"uid": self.uid}) - return ObjectState.from_json(j) - - @object_state.setter - @accepts(ObjectState) - def object_state(self, object_state): - self.remote.command("controllable/object_state/set", { - "uid": self.uid, - "state": object_state.to_json() - }) - - @property - def current_state(self): - j = self.remote.command("controllable/current_state/get", {"uid": self.uid}) - return j["state"] - - @property - def control_policy(self): - j = self.remote.command("controllable/control_policy/get", {"uid": self.uid}) - return j["control_policy"] - - @accepts(str) - def control(self, control_policy): - self.remote.command("controllable/control_policy/set", { - "uid": self.uid, - "control_policy": control_policy, - }) - - def __eq__(self, other): - return self.uid == other.uid - - def __hash__(self): - return hash(self.uid) - - def __repr__(self): - return str({ - "type": str(self.type), - "transform": str(self.transform), - "valid_actions": str(self.valid_actions), - "default_control_policy": str(self.default_control_policy), - }) + def __init__(self, remote, j): + self.remote = remote + self.uid = j["uid"] + self.type = j["type"] + self.transform = Transform.from_json(j) + self.valid_actions = j["valid_actions"] + self.default_control_policy = j["default_control_policy"] + + @property + def object_state(self): + j = self.remote.command("controllable/object_state/get", {"uid": self.uid}) + return ObjectState.from_json(j) + + @object_state.setter + @accepts(ObjectState) + def object_state(self, object_state): + self.remote.command("controllable/object_state/set", { + "uid": self.uid, + "state": object_state.to_json() + }) + + @property + def current_state(self): + j = self.remote.command("controllable/current_state/get", {"uid": self.uid}) + return j["state"] + + @property + def control_policy(self): + j = self.remote.command("controllable/control_policy/get", {"uid": self.uid}) + return j["control_policy"] + + @accepts((str, list)) + def control(self, control_policy): + self.remote.command("controllable/control_policy/set", { + "uid": self.uid, + "control_policy": control_policy, + }) + + def __eq__(self, other): + return self.uid == other.uid + + def __hash__(self): + return hash(self.uid) + + def __repr__(self): + return str({ + "type": str(self.type), + "transform": str(self.transform), + "valid_actions": str(self.valid_actions), + "default_control_policy": str(self.default_control_policy), + }) diff --git a/lgsvl/dreamview/__init__.py b/lgsvl/dreamview/__init__.py new file mode 100644 index 0000000..baf9390 --- /dev/null +++ b/lgsvl/dreamview/__init__.py @@ -0,0 +1,7 @@ +# +# Copyright (c) 2019-2020 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from .dreamview import Connection, CoordType diff --git a/lgsvl/dreamview/dreamview.py b/lgsvl/dreamview/dreamview.py new file mode 100644 index 0000000..5115a94 --- /dev/null +++ b/lgsvl/dreamview/dreamview.py @@ -0,0 +1,360 @@ +# +# Copyright (c) 2019-2020 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from websocket import create_connection +from enum import Enum +import json +import lgsvl +import math +import logging +import sys +import os + +log = logging.getLogger(__name__) + + +class CoordType(Enum): + Unity = 1 + Northing = 2 + Latitude = 3 + + +class Connection: + def __init__(self, simulator, ego_agent, ip=os.environ.get("LGSVL__AUTOPILOT_0_HOST", "localhost"), port="8888"): + """ + simulator: is an lgsvl.Simulator object + ego_agent: an lgsvl.EgoVehicle object, this is intended to be used with a vehicle equipped with Apollo 5.0 + ip: address of the machine where the Apollo stack is running + port: the port number for Dreamview + """ + self.url = "ws://" + ip + ":" + port + "/websocket" + self.sim = simulator + self.ego = ego_agent + self.ws = create_connection(self.url) + self.gps_offset = lgsvl.Vector() + + def set_destination(self, x_long_east, z_lat_north, y=0, coord_type=CoordType.Unity): + """ + This function can accept a variety of Coordinate systems + + If using Unity World Coordinate System: + x_long_east = x + z_lat_north = z + y = y + + If using Latitude/Longitude: + x_long_east = Longitude + z_lat_north = Latitude + + If using Easting/Northing: + x_long_east = Easting + z_lat_north = Northing + """ + current_pos = self.ego.state.transform + current_gps = self.sim.map_to_gps(current_pos) + heading = math.radians(current_gps.orientation) + + # Start position should be the position of the GPS + # Unity returns the position of the center of the vehicle so adjustment is required + northing_adjustment = ( + math.sin(heading) * self.gps_offset.z - math.cos(heading) * self.gps_offset.x + ) + easting_adjustment = ( + math.cos(heading) * self.gps_offset.z + math.sin(heading) * self.gps_offset.x + ) + + if coord_type == CoordType.Unity: + transform = lgsvl.Transform( + lgsvl.Vector(x_long_east, y, z_lat_north), lgsvl.Vector(0, 0, 0) + ) + gps = self.sim.map_to_gps(transform) + dest_x = gps.easting + dest_y = gps.northing + + elif coord_type == CoordType.Northing: + dest_x = x_long_east + dest_y = z_lat_north + + elif coord_type == CoordType.Latitude: + transform = self.sim.map_from_gps(longitude=x_long_east, latitude=z_lat_north) + gps = self.sim.map_to_gps(transform) + dest_x = gps.easting + dest_y = gps.northing + + else: + log.error( + "Please input a valid Coordinate Type: Unity position, Easting/Northing, or Longitude/Latitude" + ) + return + + self.ws.send( + json.dumps( + { + "type": "SendRoutingRequest", + "start": { + "x": current_gps.easting + easting_adjustment, + "y": current_gps.northing + northing_adjustment, + "z": 0, + "heading": heading, + }, + "end": {"x": dest_x, "y": dest_y, "z": 0}, + "waypoint": "[]", + } + ) + ) + + return + + def enable_module(self, module): + """ + module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview + """ + self.ws.send( + json.dumps({"type": "HMIAction", "action": "START_MODULE", "value": module}) + ) + return + + def disable_module(self, module): + """ + module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview + """ + self.ws.send( + json.dumps({"type": "HMIAction", "action": "STOP_MODULE", "value": module}) + ) + return + + def set_hd_map(self, hd_map): + """ + Folders in /apollo/modules/map/data/ are the available HD maps + Map options in Dreamview are the folder names with the following changes: + - underscores (_) are replaced with spaces + - the first letter of each word is capitalized + + hd_map parameter is the modified folder name. + hd_map should match one of the options in the right-most drop down in the top-right corner of Dreamview. + """ + + word_list = [] + for s in hd_map.split('_'): + word_list.append(s[0].upper() + s[1:]) + + mapped_map = ' '.join(word_list) + + self.ws.send( + json.dumps({"type": "HMIAction", "action": "CHANGE_MAP", "value": mapped_map}) + ) + + if not self.get_current_map() == mapped_map: + folder_name = hd_map.replace(" ", "_") + error_message = ( + "HD Map {0} was not set. Verify the files exist in " + "/apollo/modules/map/data/{1} and restart Dreamview -- Aborting..." + ) + log.error( + error_message.format( + mapped_map, folder_name + ) + ) + sys.exit(1) + return + + def set_vehicle(self, vehicle, gps_offset_x=0.0, gps_offset_y=0.0, gps_offset_z=-1.348): + # Lincoln2017MKZ from LGSVL has a GPS offset of 1.348m behind the center of the vehicle, lgsvl.Vector(0.0, 0.0, -1.348) + """ + Folders in /apollo/modules/calibration/data/ are the available vehicle calibrations + Vehicle options in Dreamview are the folder names with the following changes: + - underscores (_) are replaced with spaces + - the first letter of each word is capitalized + + vehicle parameter is the modified folder name. + vehicle should match one of the options in the middle drop down in the top-right corner of Dreamview. + """ + + word_list = [] + for s in vehicle.split('_'): + word_list.append(s[0].upper() + s[1:]) + + mapped_vehicle = ' '.join(word_list) + + self.ws.send( + json.dumps( + {"type": "HMIAction", "action": "CHANGE_VEHICLE", "value": mapped_vehicle} + ) + ) + + self.gps_offset = lgsvl.Vector(gps_offset_x, gps_offset_y, gps_offset_z) + + if not self.get_current_vehicle() == mapped_vehicle: + folder_name = vehicle.replace(" ", "_") + error_message = ( + "Vehicle calibration {0} was not set. Verify the files exist in " + "/apollo/modules/calibration/data/{1} and restart Dreamview -- Aborting..." + ) + log.error( + error_message.format( + mapped_vehicle, folder_name + ) + ) + sys.exit(1) + return + + def set_setup_mode(self, mode): + """ + mode is the name of the Apollo 5.0 mode as seen in the left-most drop down in the top-right corner of Dreamview + """ + self.ws.send( + json.dumps({"type": "HMIAction", "action": "CHANGE_MODE", "value": mode}) + ) + return + + def get_module_status(self): + """ + Returns a dict where the key is the name of the module and value is a bool based on the module's current status + """ + self.reconnect() + data = json.loads( + self.ws.recv() + ) # This first recv() call returns the SimControlStatus in the form '{"enabled":false,"type":"SimControlStatus"}' + while data["type"] != "HMIStatus": + data = json.loads(self.ws.recv()) + + # The second recv() call also contains other information: + # the current map, vehicle, and mode: + # data["data"]["currentMap"], data["data"]["currentVehicle"], data["data"]["currentMode"] + # + # the available maps, vehicles, and modes: + # data["data"]["maps"], data["data"]["vehicles"], data["data"]["modes"] + # + # the status of monitors components: + # data["data"]["monitoredComponents"] + + return data["data"]["modules"] + + def get_current_map(self): + """ + Returns the current HD Map loaded in Dreamview + """ + try: + self.reconnect() + except ConnectionRefusedError as e: + log.error("Not able to get the current HD map loaded in Dreamview.") + log.error("Original exception: " + str(e)) + return None + + data = json.loads(self.ws.recv()) + while data["type"] != "HMIStatus": + data = json.loads(self.ws.recv()) + return data["data"]["currentMap"] + + def get_current_vehicle(self): + """ + Returns the current Vehicle configuration loaded in Dreamview + """ + try: + self.reconnect() + except ConnectionRefusedError as e: + log.error("Not able to get the current vehicle configuration loaded in Dreamview.") + log.error("Original exception: " + str(e)) + return None + + data = json.loads(self.ws.recv()) + while data["type"] != "HMIStatus": + data = json.loads(self.ws.recv()) + return data["data"]["currentVehicle"] + + def reconnect(self): + """ + Closes the websocket connection and re-creates it so that data can be received again + """ + self.ws.close() + self.ws = create_connection(self.url) + return + + def enable_apollo(self, dest_x, dest_z, modules, coord_type=CoordType.Unity): + """ + Enables a list of modules and then sets the destination + """ + for mod in modules: + log.info("Starting {} module...".format(mod)) + self.enable_module(mod) + + self.set_destination(dest_x, dest_z, coord_type=coord_type) + + def disable_apollo(self): + """ + Disables all Apollo modules + """ + module_status = self.get_module_status() + for module in module_status.keys(): + self.disable_module(module) + + def check_module_status(self, modules): + """ + Checks if all modules in a provided list are enabled + """ + module_status = self.get_module_status() + for module, status in module_status.items(): + if not status and module in modules: + log.warning( + "Warning: Apollo module {} is not running!!!".format(module) + ) + + def setup_apollo(self, dest_x, dest_z, modules, default_timeout=60.0, coord_type=CoordType.Unity): + """ + Starts a list of Apollo modules and sets the destination. Will wait for Control module to send a message before returning. + Control sending a message indicates that all modules are working and Apollo is ready to continue. + """ + initial_state = self.ego.state + mod_status = self.get_module_status() + + # If any module is not enabled, Control may still send a message even though Apollo is not ready + if not all(mod_status[mod] for mod in modules): + self.disable_apollo() + + self.enable_apollo(dest_x, dest_z, modules, coord_type=coord_type) + self.ego.is_control_received = False + + def on_control_received(agent, kind, context): + if kind == "checkControl": + agent.is_control_received = True + log.info("Control message received") + + self.ego.on_custom(on_control_received) + + try: + timeout = float(os.environ.get("LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS", default_timeout)) + except Exception: + timeout = default_timeout + log.warning("Invalid LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS, using default {0}s".format(default_timeout)) + + run_time = 2 + elapsed = 0 + while timeout <= 0.0 or float(elapsed) < timeout: + self.sim.run(run_time) + + if self.ego.is_control_received: + break + + if elapsed > 0 and elapsed % (run_time * 5) == 0: + self.check_module_status(modules) + log.info("{} seconds have passed but Ego hasn't received any control messages.".format(elapsed)) + log.info("Please also check if your route has been set correctly in Dreamview.") + + elapsed += run_time + else: + log.error("No control message from Apollo within {} seconds. Aborting...".format(timeout)) + self.disable_apollo() + raise WaitApolloError() + + self.ego.state = initial_state + + +class WaitApolloError(Exception): + """ + Raised when Apollo control message is not received in time + """ + + pass diff --git a/lgsvl/evaluator/__init__.py b/lgsvl/evaluator/__init__.py new file mode 100644 index 0000000..49ea0db --- /dev/null +++ b/lgsvl/evaluator/__init__.py @@ -0,0 +1,7 @@ +# +# Copyright (c) 2020 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from .utils import TestException, right_lane_check, separation, almost_equal, in_parking_zone diff --git a/lgsvl/evaluator/utils.py b/lgsvl/evaluator/utils.py new file mode 100644 index 0000000..bdf4dff --- /dev/null +++ b/lgsvl/evaluator/utils.py @@ -0,0 +1,42 @@ +# +# Copyright (c) 2020 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import lgsvl +import numpy + + +class TestException(Exception): + pass + + +def right_lane_check(simulator, ego_transform): + egoLane = simulator.map_point_on_lane(ego_transform.position) + right = lgsvl.utils.transform_to_right(ego_transform) + rightLane = simulator.map_point_on_lane(ego_transform.position + 3.6 * right) + + return almost_equal(egoLane.position.x, rightLane.position.x) and \ + almost_equal(egoLane.position.y, rightLane.position.y) and \ + almost_equal(egoLane.position.z, rightLane.position.z) + + +def in_parking_zone(beginning, end, ego_transform): + forward = lgsvl.utils.transform_to_forward(ego_transform) + b2e = ego_transform.position - beginning # Vector from beginning of parking zone to EGO's position + b2e = b2e * (1 / b2e.magnitude()) # Make it a Unit vector to simplify dot product result + e2e = end - ego_transform.position # Vector from EGO's position to end of parking zone + e2e = e2e * (1 / e2e.magnitude()) + return ( + numpy.dot([forward.x, forward.y, forward.z], [b2e.x, b2e.y, b2e.z]) > 0.9 + and numpy.dot([forward.x, forward.y, forward.z], [e2e.x, e2e.y, e2e.z]) > 0.9 + ) + + +def almost_equal(a, b, diff=0.5): + return abs(a - b) <= diff + + +def separation(V1, V2): + return (V1 - V2).magnitude() diff --git a/lgsvl/geometry.py b/lgsvl/geometry.py index 40f9977..d914d50 100644 --- a/lgsvl/geometry.py +++ b/lgsvl/geometry.py @@ -1,106 +1,152 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # + from math import sqrt -import math class Vector: - def __init__(self, x = 0.0, y = 0.0, z = 0.0): - self.x = x - self.y = y - self.z = z - - @staticmethod - def from_json(j): - return Vector(j["x"], j["y"], j["z"]) - - def to_json(self): - return {"x": self.x, "y": self.y, "z": self.z} - - def __repr__(self): - return "Vector({}, {}, {})".format(self.x, self.y, self.z) - - def __add__(self, v): - if isinstance(v, Vector): - return Vector(self.x + v.x, self.y + v.y, self.z + v.z) - elif isinstance(v, (int, float)): - return Vector(self.x + v, self.y + v, self.z + v) - else: - raise TypeError("Vector addition only allowed between Vectors and numbers") - - def __sub__(self, v): - if isinstance(v, Vector): - return Vector(self.x - v.x, self.y - v.y, self.z - v.z) - elif isinstance(v, (int, float)): - return Vector(self.x - v, self.y - v, self.z - v) - else: - raise TypeError("Vector subtraction only allowed between Vectors and numbers") - - def __mul__(self, v): - if isinstance(v, Vector): - return Vector(self.x * v.x, self.y * v.y, self.z * v.z) - elif isinstance(v, (int, float)): - return Vector(self.x * v, self.y * v, self.z * v) - else: - raise TypeError("Vector multiplication only allowed between Vectors and numbers") - - def __rmul__(self, v): - return self * v - - def __neg__(self): - return Vector(-self.x, -self.y, -self.z) - - def magnitude(self): - return sqrt(self.x**2 + self.y**2 + self.z**2) + def __init__(self, x=0.0, y=0.0, z=0.0): + self.x = x + self.y = y + self.z = z + + @staticmethod + def from_json(j): + return Vector(j["x"], j["y"], j["z"]) + + def to_json(self): + return {"x": self.x, "y": self.y, "z": self.z} + + def __repr__(self): + return "Vector({}, {}, {})".format(self.x, self.y, self.z) + + def __add__(self, v): + if isinstance(v, Vector): + return Vector(self.x + v.x, self.y + v.y, self.z + v.z) + elif isinstance(v, (int, float)): + return Vector(self.x + v, self.y + v, self.z + v) + else: + raise TypeError("Vector addition only allowed between Vectors and numbers") + + def __sub__(self, v): + if isinstance(v, Vector): + return Vector(self.x - v.x, self.y - v.y, self.z - v.z) + elif isinstance(v, (int, float)): + return Vector(self.x - v, self.y - v, self.z - v) + else: + raise TypeError("Vector subtraction only allowed between Vectors and numbers") + + def __mul__(self, v): + if isinstance(v, Vector): + return Vector(self.x * v.x, self.y * v.y, self.z * v.z) + elif isinstance(v, (int, float)): + return Vector(self.x * v, self.y * v, self.z * v) + else: + raise TypeError("Vector multiplication only allowed between Vectors and numbers") + + def __rmul__(self, v): + return self * v + + def __neg__(self): + return Vector(-self.x, -self.y, -self.z) + + def magnitude(self): + return sqrt(self.x**2 + self.y**2 + self.z**2) + class BoundingBox: - def __init__(self, min, max): - self.min = min - self.max = max + def __init__(self, min, max): + self.min = min + self.max = max - @staticmethod - def from_json(j): - return BoundingBox(Vector.from_json(j["min"]), Vector.from_json(j["max"])) + @staticmethod + def from_json(j): + return BoundingBox(Vector.from_json(j["min"]), Vector.from_json(j["max"])) - def to_json(self): - return {"min": self.min.to_json(), "max": self.max.to_json()} + def to_json(self): + return {"min": self.min.to_json(), "max": self.max.to_json()} - def __repr__(self): - return "BoundingBox({}, {})".format(self.min, self.max) + def __repr__(self): + return "BoundingBox({}, {})".format(self.min, self.max) - @property - def center(self): - return Vector( - (self.max.x + self.min.x) * 0.5, - (self.max.y + self.min.y) * 0.5, - (self.max.z + self.min.z) * 0.5, - ) + @property + def center(self): + return Vector( + (self.max.x + self.min.x) * 0.5, + (self.max.y + self.min.y) * 0.5, + (self.max.z + self.min.z) * 0.5, + ) - @property - def size(self): - return Vector( - self.max.x - self.min.x, - self.max.y - self.min.y, - self.max.z - self.min.z, - ) + @property + def size(self): + return Vector( + self.max.x - self.min.x, + self.max.y - self.min.y, + self.max.z - self.min.z, + ) class Transform: - def __init__(self, position = None, rotation = None): - if position is None: position = Vector() - if rotation is None: rotation = Vector() - self.position = position - self.rotation = rotation - - @staticmethod - def from_json(j): - return Transform(Vector.from_json(j["position"]), Vector.from_json(j["rotation"])) - - def to_json(self): - return {"position": self.position.to_json(), "rotation": self.rotation.to_json()} - - def __repr__(self): - return "Transform(position={}, rotation={})".format(self.position, self.rotation) + def __init__(self, position=None, rotation=None): + if position is None: position = Vector() + if rotation is None: rotation = Vector() + self.position = position + self.rotation = rotation + + @staticmethod + def from_json(j): + return Transform(Vector.from_json(j["position"]), Vector.from_json(j["rotation"])) + + def to_json(self): + return {"position": self.position.to_json(), "rotation": self.rotation.to_json()} + + def __repr__(self): + return "Transform(position={}, rotation={})".format(self.position, self.rotation) + + +class Spawn: + def __init__(self, transform=None, destinations=None): + if transform is None: transform = Transform() + if destinations is None: destinations = [] + self.position = transform.position + self.rotation = transform.rotation + self.destinations = destinations + + @staticmethod + def from_json(j): + spawn_point = Transform.from_json(j) + destinations = [] + if "destinations" in j: + for d in j["destinations"]: + destinations.append(Transform.from_json(d)) + + return Spawn(spawn_point, destinations) + + def to_json(self): + return {"position": self.position.to_json(), "rotation": self.rotation.to_json()} + + def __repr__(self): + return "Spawn(position={}, rotation={}, destinations={})".format( + self.position, self.rotation, self.destinations + ) + + +class Quaternion: + def __init__(self, x=0.0, y=0.0, z=0.0, w=0.0): + self.x = x + self.y = y + self.z = z + self.w = w + + @staticmethod + def from_json(j): + return Quaternion(j["x"], j["y"], j["z"], j["w"]) + + def to_json(self): + return {"x": self.x, "y": self.y, "z": self.z, "w": self.w} + + def __repr__(self): + return "Quaternion({}, {}, {}, {})".format(self.x, self.y, self.z, self.w) diff --git a/lgsvl/remote.py b/lgsvl/remote.py index f8c35fa..5175bff 100644 --- a/lgsvl/remote.py +++ b/lgsvl/remote.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -9,61 +9,62 @@ import asyncio import json + class Remote(threading.Thread): - def __init__(self, host, port): - super().__init__(daemon=True) - self.endpoint = "ws://{}:{}".format(host, port) - self.lock = threading.Lock() - self.cv = threading.Condition() - self.data = None - self.sem = threading.Semaphore(0) - self.running = True - self.start() - self.sem.acquire() + def __init__(self, host, port): + super().__init__(daemon=True) + self.endpoint = "ws://{}:{}".format(host, port) + self.lock = threading.Lock() + self.cv = threading.Condition() + self.data = None + self.sem = threading.Semaphore(0) + self.running = True + self.start() + self.sem.acquire() - def run(self): - self.loop = asyncio.new_event_loop() - asyncio.set_event_loop(self.loop) - self.loop.run_until_complete(self.process()) + def run(self): + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + self.loop.run_until_complete(self.process()) - def close(self): - asyncio.run_coroutine_threadsafe(self.websocket.close(), self.loop) - self.join() - self.loop.close() + def close(self): + asyncio.run_coroutine_threadsafe(self.websocket.close(), self.loop) + self.join() + self.loop.close() - async def process(self): - self.websocket = await websockets.connect(self.endpoint, compression=None) - self.sem.release() + async def process(self): + self.websocket = await websockets.connect(self.endpoint, compression=None) + self.sem.release() - while True: - try: - data = await self.websocket.recv() - except Exception as e: - if isinstance(e, websockets.exceptions.ConnectionClosed): - break - with self.cv: - self.data = {"error": str(e)} - self.cv.notify() - break - with self.cv: - self.data = json.loads(data) - self.cv.notify() + while True: + try: + data = await self.websocket.recv() + except Exception as e: + if isinstance(e, websockets.exceptions.ConnectionClosed): + break + with self.cv: + self.data = {"error": str(e)} + self.cv.notify() + break + with self.cv: + self.data = json.loads(data) + self.cv.notify() - await self.websocket.close() + await self.websocket.close() - def command(self, name, args = {}): - if not self.websocket: - raise Exception("Not connected") - - data = json.dumps({"command": name, "arguments": args}) - asyncio.run_coroutine_threadsafe(self.websocket.send(data), self.loop) + def command(self, name, args={}): + if not self.websocket: + raise Exception("Not connected") - with self.cv: - self.cv.wait_for(lambda: self.data is not None) - data = self.data - self.data = None + data = json.dumps({"command": name, "arguments": args}) + asyncio.run_coroutine_threadsafe(self.websocket.send(data), self.loop) + + with self.cv: + self.cv.wait_for(lambda: self.data is not None) + data = self.data + self.data = None - if "error" in data: - raise Exception(data["error"]) - return data["result"] + if "error" in data: + raise Exception(data["error"]) + return data["result"] diff --git a/lgsvl/sensor.py b/lgsvl/sensor.py index 0ac1031..2b96a8c 100644 --- a/lgsvl/sensor.py +++ b/lgsvl/sensor.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -11,119 +11,146 @@ GpsData = namedtuple("GpsData", "latitude longitude northing easting altitude orientation") + class Sensor: - def __init__(self, remote, uid, name): - self.remote = remote - self.uid = uid - self.name = name - - @property - def transform(self): - j = self.remote.command("sensor/transform/get", {"uid": self.uid}) - return Transform.from_json(j) - - @property - def enabled(self): - return self.remote.command("sensor/enabled/get", {"uid": self.uid}) - - @enabled.setter - @accepts(bool) - def enabled(self, value): - self.remote.command("sensor/enabled/set", {"uid": self.uid, "enabled": value}) - - def __eq__(self, other): - return self.uid == other.uid - - def __hash__(self): - return hash(self.uid) - - @staticmethod - def create(remote, j): - if j["type"] == "camera": - return CameraSensor(remote, j) - if j["type"] == "lidar": - return LidarSensor(remote, j) - if j["type"] == "imu": - return ImuSensor(remote, j) - if j["type"] == "gps": - return GpsSensor(remote, j) - if j["type"] == "radar": - return RadarSensor(remote, j) - if j["type"] == "canbus": - return CanBusSensor(remote, j) - raise ValueError("Sensor type '{}' not supported".format(j["type"])) + def __init__(self, remote, uid, name): + self.remote = remote + self.uid = uid + self.name = name + + @property + def transform(self): + j = self.remote.command("sensor/transform/get", {"uid": self.uid}) + return Transform.from_json(j) + + @property + def enabled(self): + return self.remote.command("sensor/enabled/get", {"uid": self.uid}) + + @enabled.setter + @accepts(bool) + def enabled(self, value): + self.remote.command("sensor/enabled/set", {"uid": self.uid, "enabled": value}) + + def __eq__(self, other): + return self.uid == other.uid + + def __hash__(self): + return hash(self.uid) + + @staticmethod + def create(remote, j): + if j["type"] == "camera": + return CameraSensor(remote, j) + if j["type"] == "lidar": + return LidarSensor(remote, j) + if j["type"] == "imu": + return ImuSensor(remote, j) + if j["type"] == "gps": + return GpsSensor(remote, j) + if j["type"] == "radar": + return RadarSensor(remote, j) + if j["type"] == "canbus": + return CanBusSensor(remote, j) + if j["type"] == "recorder": + return VideoRecordingSensor(remote, j) + if j["type"] == "analysis": + return AnalysisSensor(remote, j) + raise ValueError("Sensor type '{}' not supported".format(j["type"])) class CameraSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) - self.frequency = j["frequency"] - self.width = j["width"] - self.height = j["height"] - self.fov = j["fov"] - self.near_plane = j["near_plane"] - self.far_plane = j["far_plane"] - """ - RGB - 24-bit color image - DEPTH - 8-bit grayscale depth buffer - SEMANTIC - 24-bit color image with semantic segmentation - """ - self.format = j["format"] - - @accepts(str, int, int) - def save(self, path, quality = 75, compression = 6): - success = self.remote.command("sensor/camera/save", { - "uid": self.uid, - "path": path, - "quality": quality, - "compression": compression, - }) - return success + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.frequency = j["frequency"] + self.width = j["width"] + self.height = j["height"] + self.fov = j["fov"] + self.near_plane = j["near_plane"] + self.far_plane = j["far_plane"] + """ + RGB - 24-bit color image + DEPTH - 8-bit grayscale depth buffer + SEMANTIC - 24-bit color image with semantic segmentation + """ + self.format = j["format"] + + @accepts(str, int, int) + def save(self, path, quality=75, compression=6): + success = self.remote.command("sensor/camera/save", { + "uid": self.uid, + "path": path, + "quality": quality, + "compression": compression, + }) + return success class LidarSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) - self.min_distance = j["min_distance"] - self.max_distance = j["max_distance"] - self.rays = j["rays"] - self.rotations = j["rotations"] # rotation frequency, Hz - self.measurements = j["measurements"] # how many measurements each ray does per one rotation - self.fov = j["fov"] - self.angle = j["angle"] - self.compensated = j["compensated"] - - @accepts(str) - def save(self, path): - success = self.remote.command("sensor/lidar/save", { - "uid": self.uid, - "path": path, - }) - return success + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.min_distance = j["min_distance"] + self.max_distance = j["max_distance"] + self.rays = j["rays"] + self.rotations = j["rotations"] # rotation frequency, Hz + self.measurements = j["measurements"] # how many measurements each ray does per one rotation + self.fov = j["fov"] + self.angle = j["angle"] + self.compensated = j["compensated"] + + @accepts(str) + def save(self, path): + success = self.remote.command("sensor/lidar/save", { + "uid": self.uid, + "path": path, + }) + return success class ImuSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) class GpsSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) - self.frequency = j["frequency"] + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.frequency = j["frequency"] - @property - def data(self): - j = self.remote.command("sensor/gps/data", {"uid": self.uid}) - return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) + @property + def data(self): + j = self.remote.command("sensor/gps/data", {"uid": self.uid}) + return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) class RadarSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) class CanBusSensor(Sensor): - def __init__(self, remote, j): - super().__init__(remote, j["uid"], j["name"]) - self.frequency = j["frequency"] \ No newline at end of file + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.frequency = j["frequency"] + + +class VideoRecordingSensor(Sensor): + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.width = j["width"] + self.height = j["height"] + self.framerate = j["framerate"] + self.min_distance = j["near_plane"] + self.max_distance = j["far_plane"] + self.fov = j["fov"] + self.quality = j["quality"] + self.bitrate = j["bitrate"] + self.max_bitrate = j["max_bitrate"] + + +class AnalysisSensor(Sensor): + def __init__(self, remote, j): + super().__init__(remote, j["uid"], j["name"]) + self.stucktravelthreshold = j["stucktravelthreshold"] + self.stucktimethreshold = j["stucktimethreshold"] + self.stoplinethreshold = j["stoplinethreshold"] diff --git a/lgsvl/simulator.py b/lgsvl/simulator.py index 2c32f15..f486f7a 100644 --- a/lgsvl/simulator.py +++ b/lgsvl/simulator.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -7,264 +7,377 @@ from .remote import Remote from .agent import Agent, AgentType, AgentState from .sensor import GpsData -from .geometry import Vector, Transform +from .geometry import Vector, Transform, Spawn, Quaternion from .utils import accepts, ObjectState from .controllable import Controllable +from enum import Enum from collections import namedtuple +from environs import Env +from datetime import datetime +import re RaycastHit = namedtuple("RaycastHit", "distance point normal") -WeatherState = namedtuple("WeatherState", "rain fog wetness") +WeatherState = namedtuple("WeatherState", "rain fog wetness cloudiness damage") +WeatherState.__new__.__defaults__ = (0,) * len(WeatherState._fields) + +env = Env() class Simulator: - @accepts(str, int) - def __init__(self, address = "localhost", port = 8181): - if port <= 0 or port > 65535: raise ValueError("port value is out of range") - self.remote = Remote(address, port) - self.agents = {} - self.callbacks = {} - self.stopped = False - - def close(self): - self.remote.close() - - @accepts(str, int) - def load(self, scene, seed=None): - self.remote.command("simulator/load_scene", {"scene": scene, "seed": seed}) - self.agents.clear() - self.callbacks.clear() - - @property - def version(self): - return self.remote.command("simulator/version") - - @property - def current_scene(self): - return self.remote.command("simulator/current_scene") - - @property - def current_frame(self): - return self.remote.command("simulator/current_frame") - - @property - def current_time(self): - return self.remote.command("simulator/current_time") - - def reset(self): - self.remote.command("simulator/reset") - self.agents.clear() - self.callbacks.clear() - - def stop(self): - self.stopped = True - - @accepts((int, float), (int, float)) - def run(self, time_limit = 0.0, time_scale = None): - self._process("simulator/run", {"time_limit": time_limit, "time_scale": time_scale}) - - def _add_callback(self, agent, name, fn): - if agent not in self.callbacks: - self.callbacks[agent] = {} - if name not in self.callbacks[agent]: - self.callbacks[agent][name] = set() - self.callbacks[agent][name].add(fn) - - def _process_events(self, events): - self.stopped = False - for ev in events: - agent = self.agents[ev["agent"]] - if agent in self.callbacks: - callbacks = self.callbacks[agent] - event_type = ev["type"] - if event_type in callbacks: - for fn in callbacks[event_type]: - if event_type == "collision": - fn(agent, self.agents.get(ev["other"]), Vector.from_json(ev["contact"]) if ev["contact"] is not None else None) - elif event_type == "waypoint_reached": - fn(agent, ev["index"]) - elif event_type == "stop_line": - fn(agent) - elif event_type == "lane_change": - fn(agent) - elif event_type == "custom": - fn(agent, ev["kind"], ev["context"]) - if self.stopped: - return - - def _process(self, cmd, args): - j = self.remote.command(cmd, args) - while True: - if j is None: - return - if "events" in j: - self._process_events(j["events"]) - if self.stopped: - break - j = self.remote.command("simulator/continue") - - @accepts(str, AgentType, AgentState) - def add_agent(self, name, agent_type, state = None): - if state is None: state = AgentState() - args = {"name": name, "type": agent_type.value, "state": state.to_json()} - uid = self.remote.command("simulator/add_agent", args) - agent = Agent.create(self, uid, agent_type) - agent.name = name - self.agents[uid] = agent - return agent - - @accepts(Agent) - def remove_agent(self, agent): - self.remote.command("simulator/agent/remove", {"uid": agent.uid}) - del self.agents[agent.uid] - if agent in self.callbacks: - del self.callbacks[agent] - - def get_agents(self): - return list(self.agents.values()) - - @property - def weather(self): - j = self.remote.command("environment/weather/get") - return WeatherState(j["rain"], j["fog"], j["wetness"]) - - @weather.setter - @accepts(WeatherState) - def weather(self, state): - self.remote.command("environment/weather/set", {"rain": state.rain, "fog": state.fog, "wetness": state.wetness}) - - @property - def time_of_day(self): - return self.remote.command("environment/time/get") - - @accepts((int, float), bool) - def set_time_of_day(self, time, fixed = True): - self.remote.command("environment/time/set", {"time": time, "fixed": fixed}) - - def get_spawn(self): - spawns = self.remote.command("map/spawn/get") - return [Transform.from_json(spawn) for spawn in spawns] - - @accepts(Transform) - def map_to_gps(self, transform): - j = self.remote.command("map/to_gps", {"transform": transform.to_json()}) - return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) - - def map_from_gps(self, latitude = None, longitude = None, northing = None, easting = None, altitude = None, orientation = None): - c = [] - coord = { - "latitude": latitude, - "longitude": longitude, - "northing": northing, - "easting": easting, - "altitude": altitude, - "orientation": orientation - } - c.append(coord) - return self.map_from_gps_batch(c)[0] - - def map_from_gps_batch(self, coords): - # coords dictionary - jarr = [] - - for c in coords: - j = {} - numtype = (int, float) - if ("latitude" in c and c["latitude"] is not None) and ("longitude" in c and c["longitude"] is not None): - if not isinstance(c["latitude"], numtype): raise TypeError("Argument 'latitude' should have '{}' type".format(numtype)) - if not isinstance(c["longitude"], numtype): raise TypeError("Argument 'longitude' should have '{}' type".format(numtype)) - if c["latitude"] < -90 or c["latitude"] > 90: raise ValueError("Latitude is out of range") - if c["longitude"] < -180 or c["longitude"] > 180: raise ValueError("Longitude is out of range") - j["latitude"] = c["latitude"] - j["longitude"] = c["longitude"] - elif ("northing" in c and c["northing"] is not None) and ("easting" in c and c["easting"] is not None): - if not isinstance(c["northing"], numtype): raise TypeError("Argument 'northing' should have '{}' type".format(numtype)) - if not isinstance(c["easting"], numtype): raise TypeError("Argument 'easting' should have '{}' type".format(numtype)) - if c["northing"] < 0 or c["northing"] > 10000000: raise ValueError("Northing is out of range") - if c["easting"] < 160000 or c["easting"] > 834000 : raise ValueError("Easting is out of range") - j["northing"] = c["northing"] - j["easting"] = c["easting"] - else: - raise Exception("Either latitude and longitude or northing and easting should be specified") - if "altitude" in c and c["altitude"] is not None: - if not isinstance(c["altitude"], numtype): raise TypeError("Argument 'altitude' should have '{}' type".format(numtype)) - j["altitude"] = c["altitude"] - if "orientation" in c and c["orientation"] is not None: - if not isinstance(c["orientation"], numtype): raise TypeError("Argument 'orientation' should have '{}' type".format(numtype)) - j["orientation"] = c["orientation"] - jarr.append(j) - - jarr = self.remote.command("map/from_gps", jarr) - transforms = [] - for j in jarr: - transforms.append(Transform.from_json(j)) - return transforms - - @accepts(Vector) - def map_point_on_lane(self, point): - j = self.remote.command("map/point_on_lane", {"point": point.to_json()}) - return Transform.from_json(j) - - @accepts(Vector, Vector, int, float) - def raycast(self, origin, direction, layer_mask = -1, max_distance = float("inf")): - hit = self.remote.command("simulator/raycast", [{ - "origin": origin.to_json(), - "direction": direction.to_json(), - "layer_mask": layer_mask, - "max_distance": max_distance - }]) - if hit[0] is None: - return None - return RaycastHit(hit[0]["distance"], Vector.from_json(hit[0]["point"]), Vector.from_json(hit[0]["normal"])) - - - def raycast_batch(self, args): - jarr = [] - for arg in args: - jarr.append({ - "origin": arg["origin"].to_json(), - "direction": arg["direction"].to_json(), - "layer_mask": arg["layer_mask"], - "max_distance": arg["max_distance"] - }) - - hits = self.remote.command("simulator/raycast", jarr) - results = [] - for hit in hits: - if hit is None: - results.append(None) - else: - results.append(RaycastHit(hit["distance"], Vector.from_json(hit["point"]), Vector.from_json(hit["normal"]))) - - return results - - @accepts(str, ObjectState) - def controllable_add(self, name, object_state = None): - if object_state is None: object_state = ObjectState() - args = {"name": name, "state": object_state.to_json()} - j = self.remote.command("simulator/controllable_add", args) - controllable = Controllable(self.remote, j) - controllable.name = name - return controllable - - @accepts(Controllable) - def controllable_remove(self, controllable): - self.remote.command("simulator/controllable_remove", {"uid": controllable.uid}) - del self.controllables[controllable.uid] - - @accepts(str) - def get_controllables(self, control_type = None): - j = self.remote.command("controllable/get/all", { - "type": control_type, - }) - return [Controllable(self.remote, controllable) for controllable in j] - - @accepts(Vector, str) - def get_controllable(self, position, control_type = None): - j = self.remote.command("controllable/get", { - "position": position.to_json(), - "type": control_type, - }) - return Controllable(self.remote, j) \ No newline at end of file + class SimulatorCameraState(Enum): + FREE = 0 + FOLLOW = 1 + CINEMATIC = 2 + DRIVER = 3 + + @accepts(str, int) + def __init__(self, address=env.str("LGSVL__SIMULATOR_HOST", "localhost"), port=env.int("LGSVL__SIMULATOR_PORT", 8181)): + if port <= 0 or port > 65535: + raise ValueError("port value is out of range") + self.remote = Remote(address, port) + self.agents = {} + self.callbacks = {} + self.stopped = False + + def close(self): + self.remote.close() + + @accepts(str, int) + def load(self, scene, seed=None): + self.remote.command("simulator/load_scene", {"scene": scene, "seed": seed}) + self.agents.clear() + self.callbacks.clear() + + @property + def version(self): + return self.remote.command("simulator/version") + + @property + def layers(self): + return self.remote.command("simulator/layers/get") + + @property + def current_scene(self): + return self.remote.command("simulator/current_scene") + + @property + def current_scene_id(self): + return self.remote.command("simulator/current_scene_id") + + @property + def current_frame(self): + return self.remote.command("simulator/current_frame") + + @property + def current_time(self): + return self.remote.command("simulator/current_time") + + @property + def available_agents(self): + return self.remote.command("simulator/available_agents") + + @property + def available_npc_behaviours(self): + return self.remote.command("simulator/npc/available_behaviours") + + @accepts(Transform) + def set_sim_camera(self, transform): + self.remote.command("simulator/camera/set", {"transform": transform.to_json()}) + + @accepts(SimulatorCameraState) + def set_sim_camera_state(self, state): + self.remote.command("simulator/camera/state/set", {"state": state.value}) + + def agents_traversed_waypoints(self, fn): + self._add_callback(None, "agents_traversed_waypoints", fn) + + def reset(self): + self.remote.command("simulator/reset") + self.agents.clear() + self.callbacks.clear() + + def stop(self): + self.stopped = True + + @accepts((int, float), (int, float)) + def run(self, time_limit=0.0, time_scale=None): + self._process("simulator/run", {"time_limit": time_limit, "time_scale": time_scale}) + + def _add_callback(self, agent, name, fn): + if agent not in self.callbacks: + self.callbacks[agent] = {} + if name not in self.callbacks[agent]: + self.callbacks[agent][name] = set() + self.callbacks[agent][name].add(fn) + + def _process_events(self, events): + self.stopped = False + for ev in events: + if "agent" in ev: + agent = self.agents[ev["agent"]] + if agent in self.callbacks: + callbacks = self.callbacks[agent] + event_type = ev["type"] + if event_type in callbacks: + for fn in callbacks[event_type]: + if event_type == "collision": + fn(agent, self.agents.get(ev["other"]), Vector.from_json(ev["contact"]) if ev["contact"] is not None else None) + elif event_type == "waypoint_reached": + fn(agent, ev["index"]) + elif event_type == "stop_line": + fn(agent) + elif event_type == "lane_change": + fn(agent) + elif event_type == "destination_reached": + fn(agent) + elif event_type == "custom": + fn(agent, ev["kind"], ev["context"]) + if self.stopped: + return + elif None in self.callbacks: + callbacks = self.callbacks[None] + event_type = ev["type"] + if event_type in callbacks: + for fn in callbacks[event_type]: + if event_type == "agents_traversed_waypoints": + fn() + + def _process(self, cmd, args): + j = self.remote.command(cmd, args) + while True: + if j is None: + return + if "events" in j: + self._process_events(j["events"]) + if self.stopped: + break + j = self.remote.command("simulator/continue") + + @accepts(str, AgentType, (AgentState, type(None)), (Vector, type(None))) + def add_agent(self, name, agent_type, state=None, color=None): + if state is None: state = AgentState() + if color is None: color = Vector(-1, -1, -1) + args = {"name": name, "type": agent_type.value, "state": state.to_json(), "color": color.to_json()} + uid = self.remote.command("simulator/add_agent", args) + agent = Agent.create(self, uid, agent_type) + agent.name = name + self.agents[uid] = agent + return agent + + @accepts(Agent) + def remove_agent(self, agent): + self.remote.command("simulator/agent/remove", {"uid": agent.uid}) + del self.agents[agent.uid] + if agent in self.callbacks: + del self.callbacks[agent] + + @accepts(AgentType) + def add_random_agents(self, agent_type): + args = {"type": agent_type.value} + self.remote.command("simulator/add_random_agents", args) + + def get_agents(self): + return list(self.agents.values()) + + @property + def weather(self): + j = self.remote.command("environment/weather/get") + return WeatherState(j.get("rain", 0), j.get("fog", 0), j.get("wetness", 0), j.get("cloudiness", 0), j.get("damage", 0)) + + @weather.setter + @accepts(WeatherState) + def weather(self, state): + self.remote.command("environment/weather/set", {"rain": state.rain, "fog": state.fog, "wetness": state.wetness, "cloudiness": state.cloudiness, "damage": state.damage}) + + @property + def time_of_day(self): + return self.remote.command("environment/time/get") + + @property + def current_datetime(self): + date_time_str = self.remote.command("simulator/datetime/get") + date_time_arr = list(map(int, re.split('[. :]', date_time_str))) + date_time = datetime( + date_time_arr[2], + date_time_arr[1], + date_time_arr[0], + date_time_arr[3], + date_time_arr[4], + date_time_arr[5] + ) + return date_time + + @accepts((int, float), bool) + def set_time_of_day(self, time, fixed=True): + self.remote.command("environment/time/set", {"time": time, "fixed": fixed}) + + @accepts(datetime, bool) + def set_date_time(self, date_time, fixed=True): + date_time = date_time.__str__() + self.remote.command("environment/datetime/set", {"datetime": date_time, "fixed": fixed}) + + def get_spawn(self): + spawns = self.remote.command("map/spawn/get") + return [Spawn.from_json(spawn) for spawn in spawns] + + @accepts((Transform, Spawn)) + def map_to_gps(self, transform): + j = self.remote.command("map/to_gps", {"transform": transform.to_json()}) + return GpsData(j["latitude"], j["longitude"], j["northing"], j["easting"], j["altitude"], j["orientation"]) + + def map_from_gps(self, latitude=None, longitude=None, northing=None, easting=None, altitude=None, orientation=None): + c = [] + coord = { + "latitude": latitude, + "longitude": longitude, + "northing": northing, + "easting": easting, + "altitude": altitude, + "orientation": orientation + } + c.append(coord) + return self.map_from_gps_batch(c)[0] + + def map_from_gps_batch(self, coords): + # coords dictionary + jarr = [] + + for c in coords: + j = {} + numtype = (int, float) + if ("latitude" in c and c["latitude"] is not None) and ("longitude" in c and c["longitude"] is not None): + if not isinstance(c["latitude"], numtype): raise TypeError("Argument 'latitude' should have '{}' type".format(numtype)) + if not isinstance(c["longitude"], numtype): raise TypeError("Argument 'longitude' should have '{}' type".format(numtype)) + if c["latitude"] < -90 or c["latitude"] > 90: raise ValueError("Latitude is out of range") + if c["longitude"] < -180 or c["longitude"] > 180: raise ValueError("Longitude is out of range") + j["latitude"] = c["latitude"] + j["longitude"] = c["longitude"] + elif ("northing" in c and c["northing"] is not None) and ("easting" in c and c["easting"] is not None): + if not isinstance(c["northing"], numtype): raise TypeError("Argument 'northing' should have '{}' type".format(numtype)) + if not isinstance(c["easting"], numtype): raise TypeError("Argument 'easting' should have '{}' type".format(numtype)) + if c["northing"] < 0 or c["northing"] > 10000000: raise ValueError("Northing is out of range") + if c["easting"] < 160000 or c["easting"] > 834000: raise ValueError("Easting is out of range") + j["northing"] = c["northing"] + j["easting"] = c["easting"] + else: + raise Exception("Either latitude and longitude or northing and easting should be specified") + if "altitude" in c and c["altitude"] is not None: + if not isinstance(c["altitude"], numtype): raise TypeError("Argument 'altitude' should have '{}' type".format(numtype)) + j["altitude"] = c["altitude"] + if "orientation" in c and c["orientation"] is not None: + if not isinstance(c["orientation"], numtype): raise TypeError("Argument 'orientation' should have '{}' type".format(numtype)) + j["orientation"] = c["orientation"] + jarr.append(j) + + jarr = self.remote.command("map/from_gps", jarr) + transforms = [] + for j in jarr: + transforms.append(Transform.from_json(j)) + return transforms + + @accepts(Vector) + def map_point_on_lane(self, point): + j = self.remote.command("map/point_on_lane", {"point": point.to_json()}) + return Transform.from_json(j) + + @accepts(Vector, Quaternion) + def map_from_nav(self, position, orientation): + res = self.remote.command( + "map/from_nav", + { + "position": position.to_json(), + "orientation": orientation.to_json() + } + ) + return Transform.from_json(res) + + @accepts(Transform, Vector) + def set_nav_origin(self, transform, offset=Vector()): + self.remote.command( + "navigation/set_origin", + { + "transform": transform.to_json(), + "offset": offset.to_json(), + } + ) + + def get_nav_origin(self): + res = self.remote.command("navigation/get_origin") + nav_origin = None + if res: + nav_origin = { + "transform": Transform.from_json(res), + "offset": res["offset"] + } + return nav_origin + + @accepts(Vector, Vector, int, float) + def raycast(self, origin, direction, layer_mask=-1, max_distance=float("inf")): + hit = self.remote.command("simulator/raycast", [{ + "origin": origin.to_json(), + "direction": direction.to_json(), + "layer_mask": layer_mask, + "max_distance": max_distance + }]) + if hit[0] is None: + return None + return RaycastHit(hit[0]["distance"], Vector.from_json(hit[0]["point"]), Vector.from_json(hit[0]["normal"])) + + def raycast_batch(self, args): + jarr = [] + for arg in args: + jarr.append({ + "origin": arg["origin"].to_json(), + "direction": arg["direction"].to_json(), + "layer_mask": arg["layer_mask"], + "max_distance": arg["max_distance"] + }) + + hits = self.remote.command("simulator/raycast", jarr) + results = [] + for hit in hits: + if hit is None: + results.append(None) + else: + results.append(RaycastHit(hit["distance"], Vector.from_json(hit["point"]), Vector.from_json(hit["normal"]))) + + return results + + @accepts(str, (ObjectState, type(None))) + def controllable_add(self, name, object_state=None): + if object_state is None: object_state = ObjectState() + args = {"name": name, "state": object_state.to_json()} + j = self.remote.command("simulator/controllable_add", args) + controllable = Controllable(self.remote, j) + controllable.name = name + return controllable + + @accepts(Controllable) + def controllable_remove(self, controllable): + self.remote.command("simulator/controllable_remove", {"uid": controllable.uid}) + del self.controllables[controllable.uid] + + @accepts(str) + def get_controllables(self, control_type=None): + j = self.remote.command("controllable/get/all", { + "type": control_type, + }) + return [Controllable(self.remote, controllable) for controllable in j] + + @accepts(str) + def get_controllable_by_uid(self, uid): + j = self.remote.command("controllable/get", { + "uid": uid, + }) + return Controllable(self.remote, j) + + @accepts(Vector, str) + def get_controllable(self, position, control_type=None): + j = self.remote.command("controllable/get", { + "position": position.to_json(), + "type": control_type, + }) + return Controllable(self.remote, j) diff --git a/lgsvl/utils.py b/lgsvl/utils.py index 2556775..83775a6 100644 --- a/lgsvl/utils.py +++ b/lgsvl/utils.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -9,150 +9,164 @@ import math import inspect + def accepts(*types): - def check_accepts(f): - assert len(types) + 1 == f.__code__.co_argcount - def new_f(*args, **kwargs): - names = inspect.getfullargspec(f)[0] - it = zip(args[1:], types, names[1:]) - for (a, t, n) in it: - if not isinstance(a, t): - raise TypeError("Argument '{}' should have '{}' type".format(n, t)) - return f(*args, **kwargs) - new_f.__name__ = f.__name__ - return new_f - return check_accepts + def check_accepts(f): + assert len(types) + 1 == f.__code__.co_argcount + + def new_f(*args, **kwargs): + names = inspect.getfullargspec(f)[0] + it = zip(args[1:], types, names[1:]) + for (a, t, n) in it: + if not isinstance(a, t): + raise TypeError("Argument '{}' should have '{}' type".format(n, t)) + return f(*args, **kwargs) + new_f.__name__ = f.__name__ + return new_f + return check_accepts + class ObjectState: - def __init__(self, transform = None, velocity = None, angular_velocity = None): - if transform is None: transform = Transform() - if velocity is None: velocity = Vector() - if angular_velocity is None: angular_velocity = Vector() - self.transform = transform - self.velocity = velocity - self.angular_velocity = angular_velocity - - @property - def position(self): - return self.transform.position - - @property - def rotation(self): - return self.transform.rotation - - @property - def speed(self): - return math.sqrt( - self.velocity.x * self.velocity.x + - self.velocity.y * self.velocity.y + - self.velocity.z * self.velocity.z) - - @staticmethod - def from_json(j): - return ObjectState( - Transform.from_json(j["transform"]), - Vector.from_json(j["velocity"]), - Vector.from_json(j["angular_velocity"]), - ) - - def to_json(self): - return { - "transform": self.transform.to_json(), - "velocity": self.velocity.to_json(), - "angular_velocity": self.angular_velocity.to_json(), - } - - def __repr__(self): - return str({ - "transform": str(self.transform), - "velocity": str(self.velocity), - "angular_velocity": str(self.angular_velocity), - }) + def __init__(self, transform=None, velocity=None, angular_velocity=None): + if transform is None: + transform = Transform() + if velocity is None: + velocity = Vector() + if angular_velocity is None: + angular_velocity = Vector() + self.transform = transform + self.velocity = velocity + self.angular_velocity = angular_velocity + + @property + def position(self): + return self.transform.position + + @property + def rotation(self): + return self.transform.rotation + + @property + def speed(self): + return math.sqrt( + self.velocity.x * self.velocity.x + + self.velocity.y * self.velocity.y + + self.velocity.z * self.velocity.z + ) + + @staticmethod + def from_json(j): + return ObjectState( + Transform.from_json(j["transform"]), + Vector.from_json(j["velocity"]), + Vector.from_json(j["angular_velocity"]), + ) + + def to_json(self): + return { + "transform": self.transform.to_json(), + "velocity": self.velocity.to_json(), + "angular_velocity": self.angular_velocity.to_json(), + } + + def __repr__(self): + return str( + { + "transform": str(self.transform), + "velocity": str(self.velocity), + "angular_velocity": str(self.angular_velocity), + } + ) + def transform_to_matrix(tr): - px = tr.position.x - py = tr.position.y - pz = tr.position.z + px = tr.position.x + py = tr.position.y + pz = tr.position.z - ax = tr.rotation.x * math.pi / 180.0 - ay = tr.rotation.y * math.pi / 180.0 - az = tr.rotation.z * math.pi / 180.0 + ax = tr.rotation.x * math.pi / 180.0 + ay = tr.rotation.y * math.pi / 180.0 + az = tr.rotation.z * math.pi / 180.0 - sx, cx = math.sin(ax), math.cos(ax) - sy, cy = math.sin(ay), math.cos(ay) - sz, cz = math.sin(az), math.cos(az) + sx, cx = math.sin(ax), math.cos(ax) + sy, cy = math.sin(ay), math.cos(ay) + sz, cz = math.sin(az), math.cos(az) + + # Unity uses left-handed coordinate system, Rz * Rx * Ry order + return [ + [sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz, 0.0], + [sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz, 0.0], + [cx * sy, -sx, cx * cy, 0.0], + [px, py, pz, 1.0], + ] - # Unity uses left-handed coordinate system, Rz * Rx * Ry order - return [ [ sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz, 0.0 ], - [ sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz, 0.0 ], - [ cx * sy, -sx, cx * cy, 0.0 ], - [ px, py, pz, 1.0 ], - ] def transform_to_forward(tr): - ax = tr.rotation.x * math.pi / 180.0 - sx, cx = math.sin(ax), math.cos(ax) + ax = tr.rotation.x * math.pi / 180.0 + sx, cx = math.sin(ax), math.cos(ax) + + ay = tr.rotation.y * math.pi / 180.0 + sy, cy = math.sin(ay), math.cos(ay) - ay = tr.rotation.y * math.pi / 180.0 - sy, cy = math.sin(ay), math.cos(ay) + return Vector(cx * sy, -sx, cx * cy) - return Vector(cx * sy, -sx, cx * cy) def transform_to_up(tr): - ax = tr.rotation.x * math.pi / 180.0 - ay = tr.rotation.y * math.pi / 180.0 - az = tr.rotation.z * math.pi / 180.0 + ax = tr.rotation.x * math.pi / 180.0 + ay = tr.rotation.y * math.pi / 180.0 + az = tr.rotation.z * math.pi / 180.0 - sx, cx = math.sin(ax), math.cos(ax) - sy, cy = math.sin(ay), math.cos(ay) - sz, cz = math.sin(az), math.cos(az) + sx, cx = math.sin(ax), math.cos(ax) + sy, cy = math.sin(ay), math.cos(ay) + sz, cz = math.sin(az), math.cos(az) - return Vector(sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz) + return Vector(sx * sy * cz - cy * sz, cx * cz, sy * sz + sx * cy * cz) -def transform_to_right(tr): - ax = tr.rotation.x * math.pi / 180.0 - ay = tr.rotation.y * math.pi / 180.0 - az = tr.rotation.z * math.pi / 180.0 - sx, cx = math.sin(ax), math.cos(ax) - sy, cy = math.sin(ay), math.cos(ay) - sz, cz = math.sin(az), math.cos(az) +def transform_to_right(tr): + ax = tr.rotation.x * math.pi / 180.0 + ay = tr.rotation.y * math.pi / 180.0 + az = tr.rotation.z * math.pi / 180.0 - return Vector(sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz) + sx, cx = math.sin(ax), math.cos(ax) + sy, cy = math.sin(ay), math.cos(ay) + sz, cz = math.sin(az), math.cos(az) + return Vector(sx * sy * sz + cy * cz, cx * sz, sx * cy * sz - sy * cz) def vector_dot(a, b): - return a.x * b.x + a.y * b.y + a.z * b.z + return a.x * b.x + a.y * b.y + a.z * b.z # this works only with transformation matrices (no scaling, no projection) def matrix_inverse(m): - x = Vector(m[0][0], m[0][1], m[0][2]) - y = Vector(m[1][0], m[1][1], m[1][2]) - z = Vector(m[2][0], m[2][1], m[2][2]) - v = Vector(m[3][0], m[3][1], m[3][2]) - a = -vector_dot(v, x) - b = -vector_dot(v, y) - c = -vector_dot(v, z) - return [ [ x.x, y.x, z.x, 0.0 ], - [ x.y, y.y, z.y, 0.0 ], - [ x.z, y.z, z.z, 0.0 ], - [ a, b, c, 1.0 ], - ] + x = Vector(m[0][0], m[0][1], m[0][2]) + y = Vector(m[1][0], m[1][1], m[1][2]) + z = Vector(m[2][0], m[2][1], m[2][2]) + v = Vector(m[3][0], m[3][1], m[3][2]) + a = -vector_dot(v, x) + b = -vector_dot(v, y) + c = -vector_dot(v, z) + return [ + [x.x, y.x, z.x, 0.0], + [x.y, y.y, z.y, 0.0], + [x.z, y.z, z.z, 0.0], + [a, b, c, 1.0], + ] def matrix_multiply(a, b): - r = [[0, 0, 0, 0] for t in range(4)] - for i in range(4): - for j in range(4): - for k in range(4): - r[i][j] += a[i][k] * b[k][j] - return r + r = [[0, 0, 0, 0] for t in range(4)] + for i in range(4): + for j in range(4): + for k in range(4): + r[i][j] += a[i][k] * b[k][j] + return r def vector_multiply(v, m): - tmp = [None] * 3 - for i in range(3): - tmp[i] = v.x * m[0][i] + v.y * m[1][i] + v.z * m[2][i] + m[3][i] - return Vector(tmp[0], tmp[1], tmp[2]) + tmp = [None] * 3 + for i in range(3): + tmp[i] = v.x * m[0][i] + v.y * m[1][i] + v.z * m[2][i] + m[3][i] + return Vector(tmp[0], tmp[1], tmp[2]) diff --git a/lgsvl/wise/__init__.py b/lgsvl/wise/__init__.py new file mode 100644 index 0000000..4266453 --- /dev/null +++ b/lgsvl/wise/__init__.py @@ -0,0 +1,7 @@ +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from .wise import DefaultAssets, SimulatorSettings diff --git a/lgsvl/wise/wise.py b/lgsvl/wise/wise.py new file mode 100644 index 0000000..29bf94a --- /dev/null +++ b/lgsvl/wise/wise.py @@ -0,0 +1,106 @@ +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + + +# Settings classes with common variables used in the quickstart and example scripts. +class SimulatorSettings: + # IP address of the endpoint running the Simulator. Default value is "127.0.0.1" + simulator_host = "127.0.0.1" + + # Port used by the Simulator for the Python API connection. Default value is 8181 + simulator_port = 8181 + + # IP address of the endpoint running the bridge. Default value is "127.0.0.1" + bridge_host = "127.0.0.1" + + # Port used by the Simulator for the bridge connection. Default value is 9090 + bridge_port = 9090 + + +class DefaultAssets: + # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "BorregasAve". + # https://wise.svlsimulator.com/maps/profile/aae03d2a-b7ca-4a88-9e41-9035287a12cc + map_borregasave = "aae03d2a-b7ca-4a88-9e41-9035287a12cc" + + # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "CubeTown". + # https://wise.svlsimulator.com/maps/profile/06773677-1ce3-492f-9fe2-b3147e126e27 + map_cubetown = "06773677-1ce3-492f-9fe2-b3147e126e27" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "SanFrancisco". + # https://wise.svlsimulator.com/maps/profile/5d272540-f689-4355-83c7-03bf11b6865f + map_sanfrancisco = "5d272540-f689-4355-83c7-03bf11b6865f" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight1LanePedestrianCrosswalk". + # https://wise.svlsimulator.com/maps/profile/a3a818b5-c66b-488a-a780-979bd5692db1 + map_straight1lanepedestriancrosswalk = "a3a818b5-c66b-488a-a780-979bd5692db1" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "SingleLaneRoad". + # https://wise.svlsimulator.com/maps/profile/a6e2d149-6a18-4b83-9029-4411d7b2e69a + map_singlelaneroad = "a6e2d149-6a18-4b83-9029-4411d7b2e69a" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight1LaneSame". + # https://wise.svlsimulator.com/maps/profile/1e2287cf-c590-4804-bcb1-18b2fd3752d1 + map_straight1lanesame = "1e2287cf-c590-4804-bcb1-18b2fd3752d1" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneSame". + # https://wise.svlsimulator.com/maps/profile/b39d3ef9-21d7-409d-851b-4c90dad80a25 + map_straight2lanesame = "b39d3ef9-21d7-409d-851b-4c90dad80a25" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneSameCurbRightIntersection". + # https://wise.svlsimulator.com/maps/profile/378edc3f-8fce-4596-87dc-7d12fc2ad743 + map_straight2lanesamecurbrightintersection = "378edc3f-8fce-4596-87dc-7d12fc2ad743" + + # Map loaded in some of the examples. Changing the map may lead to unexpected behaviour in the example scripts. Default value is "Straight2LaneOpposing". + # https://wise.svlsimulator.com/maps/profile/671868be-44f9-44a1-913c-cb0f29d12634 + map_straight2laneopposing = "671868be-44f9-44a1-913c-cb0f29d12634" + + # Map loaded in the quickstart scripts. Changing the map may lead to unexpected behaviour in the quickstart scripts. Default value is "LGSeocho". + # https://wise.svlsimulator.com/maps/profile/26546191-86e8-4b53-9432-1cecbbd95c87 + map_lgseocho = "26546191-86e8-4b53-9432-1cecbbd95c87" + + # Ego vehicle that is loaded in most of the quickstart scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0" sensor configuration. + # This includes a bridge connection if needed and also bunch of sensors including LIDAR. + # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/47b529db-0593-4908-b3e7-4b24a32a0f70 + ego_lincoln2017mkz_apollo5 = "47b529db-0593-4908-b3e7-4b24a32a0f70" + + # Ego vehicle that is loaded in quickstart/tests scripts. Default value is "Jaguar2015XE" using the "Apollo 5.0" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/c06d4932-5928-4730-8a91-ba64ac5f1813 + ego_jaguar2015xe_apollo5 = "c06d4932-5928-4730-8a91-ba64ac5f1813" + + # Ego vehicle that is loaded in quickstart/tests scripts. Default value is "Jaguar2015XE" using the "Autoware AI" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/3f4211dc-e5d7-42dc-94c5-c4832b1331bb/edit/configuration/05cbb194-d095-4a0e-ae66-ff56c331ca83 + ego_jaguae2015xe_autowareai = "05cbb194-d095-4a0e-ae66-ff56c331ca83" + + # Ego vehicle that is loaded in most of the NHTSA example scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0 (Full Analysis)" sensor configuration. + # This includes bunch of sensors that help analyze test results efficiently. + # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/22656c7b-104b-4e6a-9c70-9955b6582220 + ego_lincoln2017mkz_apollo5_full_analysis = "22656c7b-104b-4e6a-9c70-9955b6582220" + + # Ego vehicle that is loaded in most of the NHTSA example scripts. Default value is "Lincoln2017MKZ" using the "Apollo 5.0 (modular testing)" sensor configuration. + # This has sensors for modular testing. + # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/5c7fb3b0-1fd4-4943-8347-f73a05749718 + ego_lincoln2017mkz_apollo5_modular = "5c7fb3b0-1fd4-4943-8347-f73a05749718" + + # Ego vehicle that is loaded in most of the Apollo 6.0 scripts. Default value is "Lincoln2017MKZ" using the "Apollo 6.0 (modular testing)" sensor configuration. + # This has sensors for modular testing. + # https://wise.svlsimulator.com/vehicles/profile/73805704-1e46-4eb6-b5f9-ec2244d5951e/edit/configuration/2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921 + ego_lincoln2017mkz_apollo6_modular = "2e9095fa-c9b9-4f3f-8d7d-65fa2bb03921" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Navigation2" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/c2207cd4-c8d0-4a12-b5b7-c79ab748becc + ego_lgcloi_navigation2 = "c2207cd4-c8d0-4a12-b5b7-c79ab748becc" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot1" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/eee61d18-c6e3-4292-988d-445802aaee97 + ego_lgcloi_navigation2_multi_robot1 = "eee61d18-c6e3-4292-988d-445802aaee97" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot2" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/f9c5ace0-969a-4ade-8208-87d09d1a53f8 + ego_lgcloi_navigation2_multi_robot2 = "f9c5ace0-969a-4ade-8208-87d09d1a53f8" + + # Cloi robot that is loaded in quickstart/tests scripts. Default value is "LGCloi" using the "Nav2_Multi_Robot3" sensor configuration. + # https://wise.svlsimulator.com/vehicles/profile/20609b67-6dbd-40ad-9b46-e6bc455278ed/edit/configuration/cfdb1484-91b7-4f27-b729-e313cc31ed8e + ego_lgcloi_navigation2_multi_robot3 = "cfdb1484-91b7-4f27-b729-e313cc31ed8e" diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..20ed2e2 --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + lgsvl + 2021.1.0 + A Python API for SVL Simulator + Hadi Tabatabaee + Other + + python3-environs-pip + python3-numpy + python3-websocket + python3-websockets + + + ament_python + + diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..020a6ad --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +# get_version() in setup.py requires GitPython. +requires = ["setuptools", "wheel", "GitPython==3.1.14"] diff --git a/quickstart/01-connecting-to-simulator.py b/quickstart/01-connecting-to-simulator.py index 220b7fb..01ab56e 100755 --- a/quickstart/01-connecting-to-simulator.py +++ b/quickstart/01-connecting-to-simulator.py @@ -1,15 +1,20 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -# Connects to the simulator instance at the ip defined by SIMULATOR_HOST, default is localhost or 127.0.0.1 -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) +print("Python API Quickstart #1: Connecting to the Simulator") +env = Env() + +# Connects to the simulator instance at the ip defined by LGSVL__SIMULATOR_HOST, default is localhost or 127.0.0.1 +# env.str() is equivalent to os.environ.get() +# env.int() is equivalent to int(os.environ.get()) +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) print("Version =", sim.version) print("Current Time =", sim.current_time) diff --git a/quickstart/02-loading-scene-show-spawns.py b/quickstart/02-loading-scene-show-spawns.py index 3d50f50..c9a56f5 100755 --- a/quickstart/02-loading-scene-show-spawns.py +++ b/quickstart/02-loading-scene-show-spawns.py @@ -1,26 +1,30 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) +print("Python API Quickstart #2: Loading the scene spawn points") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) print("Current Scene = {}".format(sim.current_scene)) # Loads the named map in the connected simulator. The available maps can be set up in web interface -if sim.current_scene == "BorregasAve": - sim.reset() +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) print("Current Scene = {}".format(sim.current_scene)) +print("Current Scene ID = {}".format(sim.current_scene_id)) # This will print out the position and rotation vectors for each of the spawn points in the loaded map spawns = sim.get_spawn() for spawn in sim.get_spawn(): - print(spawn) + print(spawn) diff --git a/quickstart/03-raycast.py b/quickstart/03-raycast.py index d745693..ae254b0 100755 --- a/quickstart/03-raycast.py +++ b/quickstart/03-raycast.py @@ -1,18 +1,21 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #3: Using raycasts in the Simulator") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) # The next few lines spawns an EGO vehicle in the map spawns = sim.get_spawn() @@ -22,46 +25,50 @@ forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) up = lgsvl.utils.transform_to_up(state.transform) -sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # This is the point from which the rays will originate from. It is raised 1m from the ground p = spawns[0].position p.y += 1 +# use layers property to get all layers used in simulator # useful bits in layer mask # 0 - Default (road & ground) # 9 - EGO vehicles # 10 - NPC vehicles # 11 - Pedestrian # 12 - Obstacle +layers = sim.layers +print(layers) # Included layers can be hit by the rays. Otherwise the ray will go through the layer layer_mask = 0 -for bit in [0, 10, 11, 12]: # do not put 9 here, to not hit EGO vehicle itself - layer_mask |= 1 << bit +tohitlayers = ["Default", "NPC", "Pedestrian", "Obstacle"] +for layer in tohitlayers: + layer_mask |= 1 << layers[layer] # raycast returns None if the ray doesn't collide with anything # hit also has the point property which is the Unity position vector of where the ray collided with something hit = sim.raycast(p, right, layer_mask) if hit: - print("Distance right:", hit.distance) + print("Distance right:", hit.distance) hit = sim.raycast(p, -right, layer_mask) if hit: - print("Distance left:", hit.distance) + print("Distance left:", hit.distance) hit = sim.raycast(p, -forward, layer_mask) if hit: - print("Distance back:", hit.distance) + print("Distance back:", hit.distance) hit = sim.raycast(p, forward, layer_mask) if hit: - print("Distance forward:", hit.distance) + print("Distance forward:", hit.distance) hit = sim.raycast(p, up, layer_mask) if hit: - print("Distance up:", hit.distance) + print("Distance up:", hit.distance) hit = sim.raycast(p, -up, layer_mask) if hit: - print("Distance down:", hit.distance) + print("Distance down:", hit.distance) diff --git a/quickstart/04-ego-drive-straight.py b/quickstart/04-ego-drive-straight.py index 61d91d1..656b900 100755 --- a/quickstart/04-ego-drive-straight.py +++ b/quickstart/04-ego-drive-straight.py @@ -1,19 +1,21 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import math -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #4: Ego vehicle driving straight") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() @@ -24,10 +26,10 @@ # Agents can be spawned with a velocity. Default is to spawn with 0 velocity state.velocity = 20 * forward -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # The bounding box of an agent are 2 points (min and max) such that the box formed from those 2 points completely encases the agent -print("Vehicle bounding box =", a.bounding_box) +print("Vehicle bounding box =", ego.bounding_box) print("Current time = ", sim.current_time) print("Current frame = ", sim.current_frame) @@ -35,14 +37,14 @@ input("Press Enter to drive forward for 2 seconds") # The simulator can be run for a set amount of time. time_limit is optional and if omitted or set to 0, then the simulator will run indefinitely -sim.run(time_limit = 2.0) +sim.run(time_limit=2.0) print("Current time = ", sim.current_time) print("Current frame = ", sim.current_frame) input("Press Enter to continue driving for 2 seconds") -sim.run(time_limit = 2.0) +sim.run(time_limit=2.0) print("Current time = ", sim.current_time) print("Current frame = ", sim.current_frame) diff --git a/quickstart/05-ego-drive-in-circle.py b/quickstart/05-ego-drive-in-circle.py index 8264bcb..b32385c 100755 --- a/quickstart/05-ego-drive-in-circle.py +++ b/quickstart/05-ego-drive-in-circle.py @@ -1,31 +1,34 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #5: Ego vehicle driving in circle") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] forward = lgsvl.utils.transform_to_forward(spawns[0]) -state.transform.position += 5 * forward# 5m forwards -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +state.transform.position += 5 * forward # 5m forwards +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) print("Current time = ", sim.current_time) print("Current frame = ", sim.current_frame) -input("Press Enter to start driving") +input("Press Enter to start driving for 30 seconds") # VehicleControl objects can only be applied to EGO vehicles # You can set the steering (-1 ... 1), throttle and braking (0 ... 1), handbrake and reverse (bool) @@ -33,6 +36,6 @@ c.throttle = 0.3 c.steering = -1.0 # a True in apply_control means the control will be continuously applied ("sticky"). False means the control will be applied for 1 frame -a.apply_control(c, True) +ego.apply_control(c, True) -sim.run() +sim.run(30) diff --git a/quickstart/06-save-camera-image.py b/quickstart/06-save-camera-image.py index cd3a912..4e90274 100755 --- a/quickstart/06-save-camera-image.py +++ b/quickstart/06-save-camera-image.py @@ -1,30 +1,33 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #6: Saving an image from the Main Camera sensor") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # get_sensors returns a list of sensors on the EGO vehicle -sensors = a.get_sensors() +sensors = ego.get_sensors() for s in sensors: - if s.name == "Main Camera": - # Camera and LIDAR sensors can save data to the specified file path - s.save("main-camera.png", compression=0) - s.save("main-camera.jpg", quality=75) - break + if s.name == "Main Camera": + # Camera and LIDAR sensors can save data to the specified file path + s.save("main-camera.png", compression=0) + s.save("main-camera.jpg", quality=75) + break diff --git a/quickstart/07-save-lidar-point-cloud.py b/quickstart/07-save-lidar-point-cloud.py index 23038ef..2842c86 100755 --- a/quickstart/07-save-lidar-point-cloud.py +++ b/quickstart/07-save-lidar-point-cloud.py @@ -1,27 +1,30 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #7: Saving a point cloud from the Lidar sensor") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -sensors = a.get_sensors() +sensors = ego.get_sensors() for s in sensors: - if s.name == "Lidar": - s.save("lidar.pcd") - break + if s.name == "Lidar": + s.save("lidar.pcd") + break diff --git a/quickstart/08-create-npc.py b/quickstart/08-create-npc.py index 4d68c1e..475bc51 100755 --- a/quickstart/08-create-npc.py +++ b/quickstart/08-create-npc.py @@ -1,25 +1,27 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #8: Creating various NPCs") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) @@ -28,9 +30,9 @@ # The first will be created in front of the EGO and then they will be created to the left # The available types of NPCs can be found in NPCManager prefab for i, name in enumerate(["Sedan", "SUV", "Jeep", "Hatchback"]): - state = lgsvl.AgentState() + state = lgsvl.AgentState() - # Spawn NPC vehicles 10 meters ahead of the EGO - state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right) - state.transform.rotation = spawns[0].rotation - sim.add_agent(name, lgsvl.AgentType.NPC, state) + # Spawn NPC vehicles 10 meters ahead of the EGO + state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right) + state.transform.rotation = spawns[0].rotation + sim.add_agent(name, lgsvl.AgentType.NPC, state) diff --git a/quickstart/09-reset-scene.py b/quickstart/09-reset-scene.py index c03dc02..9c60fc0 100755 --- a/quickstart/09-reset-scene.py +++ b/quickstart/09-reset-scene.py @@ -1,40 +1,40 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import random -import time -import math +from environs import Env +import lgsvl + +print("Python API Quickstart #9: Resetting the scene to it's initial state") +env = Env() random.seed(0) -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) - for i, name in enumerate(["Sedan", "SUV", "Jeep", "Hatchback"]): - state = lgsvl.AgentState() - # 10 meters ahead - state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right) - state.transform.rotation = spawns[0].rotation - sim.add_agent(name, lgsvl.AgentType.NPC, state) + state = lgsvl.AgentState() + # 10 meters ahead + state.transform.position = spawns[0].position + (10 * forward) - (4.0 * i * right) + state.transform.rotation = spawns[0].rotation + sim.add_agent(name, lgsvl.AgentType.NPC, state) input("Press Enter to reset") diff --git a/quickstart/10-npc-follow-the-lane.py b/quickstart/10-npc-follow-the-lane.py index 85cdbf9..1b6a4af 100755 --- a/quickstart/10-npc-follow-the-lane.py +++ b/quickstart/10-npc-follow-the-lane.py @@ -1,24 +1,27 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #10: NPC following the lane") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) state = lgsvl.AgentState() @@ -37,7 +40,7 @@ state.transform.position = spawns[0].position + 4.0 * right + 10.0 * forward state.transform.rotation = spawns[0].rotation -npc2 = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) +npc2 = sim.add_agent("SUV", lgsvl.AgentType.NPC, state, lgsvl.Vector(1, 1, 0)) # If the passed bool is False, then the NPC will not moved # The float passed is the maximum speed the NPC will drive @@ -47,6 +50,6 @@ # 5.6 m/s is ~20 km/h npc2.follow_closest_lane(True, 5.6) -input("Press Enter to run") +input("Press Enter to run the simulation for 40 seconds") -sim.run() +sim.run(40) diff --git a/quickstart/11-collision-callbacks.py b/quickstart/11-collision-callbacks.py index 5a06626..3357f4e 100755 --- a/quickstart/11-collision-callbacks.py +++ b/quickstart/11-collision-callbacks.py @@ -1,36 +1,33 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #11: Handling the collision callbacks") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() - -sx = spawns[0].position.x -sz = spawns[0].position.z -ry = spawns[0].rotation.y +forward = lgsvl.utils.transform_to_forward(spawns[0]) +right = lgsvl.utils.transform_to_right(spawns[0]) # ego vehicle - state = lgsvl.AgentState() state.transform = spawns[0] -ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) - -forward = lgsvl.utils.transform_to_forward(spawns[0]) -right = lgsvl.utils.transform_to_right(spawns[0]) +state.velocity = 6 * forward +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # school bus, 20m ahead, perpendicular to road, stopped - state = lgsvl.AgentState() state.transform.position = spawns[0].position + 20.0 * forward state.transform.rotation.y = spawns[0].rotation.y + 90.0 @@ -42,26 +39,28 @@ state.transform.rotation = spawns[0].rotation sedan = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) # Even though the sedan is commanded to follow the lane, obstacle avoidance takes precedence and it will not drive into the bus -sedan.follow_closest_lane(True, 11.1) # 11.1 m/s is ~40 km/h +sedan.follow_closest_lane(True, 11.1) # 11.1 m/s is ~40 km/h vehicles = { - ego: "EGO", - bus: "SchoolBus", - sedan: "Sedan", + ego: "EGO", + bus: "SchoolBus", + sedan: "Sedan", } + # This function gets called whenever any of the 3 vehicles above collides with anything def on_collision(agent1, agent2, contact): - name1 = vehicles[agent1] - name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" - print("{} collided with {} at {}".format(name1, name2, contact)) + name1 = vehicles[agent1] + name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {} at {}".format(name1, name2, contact)) + # The above on_collision function needs to be added to the callback list of each vehicle ego.on_collision(on_collision) bus.on_collision(on_collision) sedan.on_collision(on_collision) -input("Press Enter to run") +input("Press Enter to run the simulation for 10 seconds") -# Manually drive into the sedan, bus, or obstacle to get a callback -sim.run() +# Drive into the sedan, bus, or obstacle to get a callback +sim.run(10) diff --git a/quickstart/12-create-npc-on-lane.py b/quickstart/12-create-npc-on-lane.py index 5c001a6..27a89ad 100755 --- a/quickstart/12-create-npc-on-lane.py +++ b/quickstart/12-create-npc-on-lane.py @@ -1,26 +1,29 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import math import random +from environs import Env +import lgsvl + +print("Python API Quickstart #12: Creating NPCs on lanes") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) sx = spawns[0].position.x sy = spawns[0].position.y @@ -31,16 +34,17 @@ random.seed(0) -while True: - input("Press Enter to spawn NPC") +print("Spawn 10 random NPCs on lanes") +for i in range(10): + input("Press Enter to spawn NPC ({})".format(i + 1)) -# Creates a random point around the EGO - angle = random.uniform(0.0, 2*math.pi) - dist = random.uniform(mindist, maxdist) + # Creates a random point around the EGO + angle = random.uniform(0.0, 2 * math.pi) + dist = random.uniform(mindist, maxdist) - point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) + point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) -# Creates an NPC on a lane that is closest to the random point - state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(point) - sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) + # Creates an NPC on a lane that is closest to the random point + state = lgsvl.AgentState() + state.transform = sim.map_point_on_lane(point) + sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) diff --git a/quickstart/13-npc-follow-waypoints.py b/quickstart/13-npc-follow-waypoints.py index 4f87979..9f47752 100755 --- a/quickstart/13-npc-follow-waypoints.py +++ b/quickstart/13-npc-follow-waypoints.py @@ -1,55 +1,55 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl -import math import copy +from environs import Env +import lgsvl + +print("Python API Quickstart #13: NPC following waypoints") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) -# EGO +# Creating state object to define state for Ego and NPC state = lgsvl.AgentState() state.transform = spawns[0] -state2 = copy.deepcopy(state) -state2.transform.position += 50 * forward -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state2) -# NPC, 10 meters ahead +# Ego +ego_state = copy.deepcopy(state) +ego_state.transform.position += 50 * forward +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -sx = spawns[0].position.x -sy = spawns[0].position.y -sz = spawns[0].position.z + 10.0 - -state = lgsvl.AgentState() -state.transform.position = spawns[0].position + 10 * forward -state.transform.rotation = spawns[0].rotation -npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) +# NPC +npc_state = copy.deepcopy(state) +npc_state.transform.position += 10 * forward +npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, npc_state) vehicles = { - a: "EGO", - npc: "Sedan", + ego: "EGO", + npc: "Sedan", } + # Executed upon receiving collision callback -- NPC is expected to drive through colliding objects def on_collision(agent1, agent2, contact): - name1 = vehicles[agent1] - name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" - print("{} collided with {}".format(name1, name2)) + name1 = vehicles[agent1] + name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {}".format(name1, name2)) + -a.on_collision(on_collision) +ego.on_collision(on_collision) npc.on_collision(on_collision) # This block creates the list of waypoints that the NPC will follow @@ -59,32 +59,36 @@ def on_collision(agent1, agent2, contact): z_delta = 12 layer_mask = 0 -layer_mask |= 1 << 0 # 0 is the layer for the road (default) +layer_mask |= 1 << 0 # 0 is the layer for the road (default) for i in range(20): - speed = 24# if i % 2 == 0 else 12 - px = 0 - pz = (i + 1) * z_delta - # Waypoint angles are input as Euler angles (roll, pitch, yaw) - angle = spawns[0].rotation - # Raycast the points onto the ground because BorregasAve is not flat - hit = sim.raycast(spawns[0].position + pz * forward, lgsvl.Vector(0,-1,0), layer_mask) - - # NPC will wait for 1 second at each waypoint - wp = lgsvl.DriveWaypoint(hit.point, speed, angle, 1) - waypoints.append(wp) + speed = 24 # if i % 2 == 0 else 12 + px = 0 + pz = (i + 1) * z_delta + # Waypoint angles are input as Euler angles (roll, pitch, yaw) + angle = spawns[0].rotation + # Raycast the points onto the ground because BorregasAve is not flat + hit = sim.raycast( + spawns[0].position + pz * forward, lgsvl.Vector(0, -1, 0), layer_mask + ) + + # NPC will wait for 1 second at each waypoint + wp = lgsvl.DriveWaypoint(hit.point, speed, angle=angle, idle=1) + waypoints.append(wp) + # When the NPC is within 0.5m of the waypoint, this will be called def on_waypoint(agent, index): - print("waypoint {} reached".format(index)) + print("waypoint {} reached".format(index)) + # The above function needs to be added to the list of callbacks for the NPC npc.on_waypoint_reached(on_waypoint) -# The NPC needs to be given the list of waypoints. +# The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) npc.follow(waypoints) -input("Press Enter to run") +input("Press Enter to run the simulation for 30 seconds") -sim.run() +sim.run(30) diff --git a/quickstart/14-create-pedestrians.py b/quickstart/14-create-pedestrians.py index 978c180..c58084b 100755 --- a/quickstart/14-create-pedestrians.py +++ b/quickstart/14-create-pedestrians.py @@ -1,20 +1,23 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import random import time +from environs import Env +import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #14: Creating pedestrians on the map") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() @@ -22,24 +25,35 @@ state.transform = spawns[0] forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) - -sx = state.transform.position.x -sz = state.transform.position.z +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -input("Press Enter to start creating pedestrians") +input("Press Enter to create 100 pedestrians") # The list of available pedestrians can be found in PedestrianManager prefab -names = ["Bob", "EntrepreneurFemale", "Howard", "Johny", "Pamela", "Presley", "Robin", "Stephen", "Zoe"] +names = [ + "Bob", + "EntrepreneurFemale", + "Howard", + "Johny", + "Pamela", + "Presley", + "Robin", + "Stephen", + "Zoe", +] i = 0 -while True: - state = lgsvl.AgentState() - - state.transform.position = spawns[0].position + (5 + (1.0 * (i//16))) * forward + (5 - (1.0 * (i % 16))) * right - state.transform.rotation = spawns[0].rotation - name = random.choice(names) - print("({}) adding {}".format(i+1, name)) - p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) - time.sleep(0.1) - i += 1 +while i < 100: + state = lgsvl.AgentState() + + state.transform.position = ( + spawns[0].position + + (5 + (1.0 * (i // 16))) * forward + + (5 - (1.0 * (i % 16))) * right + ) + state.transform.rotation = spawns[0].rotation + name = random.choice(names) + print("({}) adding {}".format(i + 1, name)) + p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) + time.sleep(0.1) + i += 1 diff --git a/quickstart/15-pedestrian-walk-randomly.py b/quickstart/15-pedestrian-walk-randomly.py index 9b4e72b..0d9691d 100755 --- a/quickstart/15-pedestrian-walk-randomly.py +++ b/quickstart/15-pedestrian-walk-randomly.py @@ -1,33 +1,31 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -import time -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #15: Pedestrian walking randomly") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) -sx = spawns[0].position.x -sz = spawns[0].position.z - state = lgsvl.AgentState() state.transform.position = spawns[0].position + 40 * forward state.transform.rotation = spawns[0].rotation -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) state = lgsvl.AgentState() # Spawn the pedestrian in front of car diff --git a/quickstart/16-pedestrian-follow-waypoints.py b/quickstart/16-pedestrian-follow-waypoints.py index 4712413..7327891 100755 --- a/quickstart/16-pedestrian-follow-waypoints.py +++ b/quickstart/16-pedestrian-follow-waypoints.py @@ -1,21 +1,22 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl -import random -import time import math +from environs import Env +import lgsvl + +print("Python API Quickstart #16: Pedestrian following waypoints") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[1]) @@ -23,18 +24,20 @@ state = lgsvl.AgentState() state.transform = spawns[1] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # This will create waypoints in a circle for the pedestrian to follow -radius = 5 +radius = 4 count = 8 wp = [] for i in range(count): - x = radius * math.cos(i * 2 * math.pi / count) - z = radius * math.sin(i * 2 * math.pi / count) - # idle is how much time the pedestrian will wait once it reaches the waypoint - idle = 1 if i < count//2 else 0 - wp.append(lgsvl.WalkWaypoint(spawns[1].position + (8 + x) * right + z * forward, idle)) + x = radius * math.cos(i * 2 * math.pi / count) + z = radius * math.sin(i * 2 * math.pi / count) + # idle is how much time the pedestrian will wait once it reaches the waypoint + idle = 1 if i < count // 2 else 0 + wp.append( + lgsvl.WalkWaypoint(spawns[1].position + x * right + (z + 8) * forward, idle) + ) state = lgsvl.AgentState() state.transform = spawns[1] @@ -42,14 +45,16 @@ p = sim.add_agent("Pamela", lgsvl.AgentType.PEDESTRIAN, state) + def on_waypoint(agent, index): - print("Waypoint {} reached".format(index)) + print("Waypoint {} reached".format(index)) + p.on_waypoint_reached(on_waypoint) # This sends the list of waypoints to the pedestrian. The bool controls whether or not the pedestrian will continue walking (default false) p.follow(wp, True) -input("Press Enter to walk in circle") +input("Press Enter to walk in circle for 60 seconds") -sim.run() +sim.run(60) diff --git a/quickstart/17-many-pedestrians-walking.py b/quickstart/17-many-pedestrians-walking.py index e924324..55af15e 100755 --- a/quickstart/17-many-pedestrians-walking.py +++ b/quickstart/17-many-pedestrians-walking.py @@ -1,20 +1,22 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import random -import time +from environs import Env +import lgsvl + +print("Python API Quickstart #17: Many pedestrians walking simultaneously") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() @@ -22,33 +24,41 @@ state.transform = spawns[0] forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) - -sx = state.transform.position.x -sy = state.transform.position.y -sz = state.transform.position.z - -names = ["Bob", "EntrepreneurFemale", "Howard", "Johny", "Pamela", "Presley", "Robin", "Stephen", "Zoe"] +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -for i in range(20*6): - # Create peds in a block - start = spawns[0].position + (5 + (1.0 * (i//6))) * forward - (2 + (1.0 * (i % 6))) * right - end = start + 10 * forward +names = [ + "Bob", + "EntrepreneurFemale", + "Howard", + "Johny", + "Pamela", + "Presley", + "Robin", + "Stephen", + "Zoe", +] -# Give waypoints for the spawn location and 10m ahead - wp = [ lgsvl.WalkWaypoint(start, 0), - lgsvl.WalkWaypoint(end, 0), - ] +print("Creating 120 pedestrians") +for i in range(20 * 6): + # Create peds in a block + start = ( + spawns[0].position + + (5 + (1.0 * (i // 6))) * forward + - (2 + (1.0 * (i % 6))) * right + ) + end = start + 10 * forward - state = lgsvl.AgentState() - state.transform.position = start - state.transform.rotation = spawns[0].rotation - name = random.choice(names) + # Give waypoints for the spawn location and 10m ahead + wp = [lgsvl.WalkWaypoint(start, 0), lgsvl.WalkWaypoint(end, 0)] -# Send the waypoints and make the pedestrian loop over the waypoints - p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) - p.follow(wp, True) + state = lgsvl.AgentState() + state.transform.position = start + state.transform.rotation = spawns[0].rotation + name = random.choice(names) -input("Press Enter to start") + # Send the waypoints and make the pedestrian loop over the waypoints + p = sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) + p.follow(wp, True) -sim.run() +input("Press Enter to start the simulation for 30 seconds") +sim.run(30) diff --git a/quickstart/18-weather-effects.py b/quickstart/18-weather-effects.py index 0b031eb..c62f281 100755 --- a/quickstart/18-weather-effects.py +++ b/quickstart/18-weather-effects.py @@ -1,55 +1,56 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -import time -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #18: Changing the weather effects") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) print(sim.weather) input("Press Enter to set rain to 80%") -# Each weather variable is a float from 0 to 1 +# Each weather variable is a float from 0 to 1 # There is no default value so each varible must be specified -sim.weather = lgsvl.WeatherState(rain=0.8, fog=0, wetness=0) +sim.weather = lgsvl.WeatherState(rain=0.8, fog=0, wetness=0, cloudiness=0, damage=0) print(sim.weather) sim.run(5) input("Press Enter to set fog to 50%") -sim.weather = lgsvl.WeatherState(rain=0, fog=0.5, wetness=0) +sim.weather = lgsvl.WeatherState(rain=0, fog=0.5, wetness=0, cloudiness=0, damage=0) print(sim.weather) sim.run(5) input("Press Enter to set wetness to 50%") -sim.weather = lgsvl.WeatherState(rain=0, fog=0, wetness=0.5) +sim.weather = lgsvl.WeatherState(rain=0, fog=0, wetness=0.5, cloudiness=0, damage=0) print(sim.weather) sim.run(5) input("Press Enter to reset to 0") -sim.weather = lgsvl.WeatherState(rain=0, fog=0, wetness=0) +sim.weather = lgsvl.WeatherState(rain=0, fog=0, wetness=0, cloudiness=0, damage=0) print(sim.weather) sim.run(5) diff --git a/quickstart/19-time-of-day.py b/quickstart/19-time-of-day.py index a0597db..70f230d 100755 --- a/quickstart/19-time-of-day.py +++ b/quickstart/19-time-of-day.py @@ -1,26 +1,28 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -import time +from datetime import datetime -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #19: Changing the time of day") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) print("Current time:", sim.time_of_day) @@ -40,3 +42,14 @@ sim.run(5) print(sim.time_of_day) + +input("Press Enter to set date to July 1 2020 and time to 19:00") +dt = datetime(2020, 7, 1, 19, 0, 0, 0) +sim.set_date_time(dt, False) +print(sim.time_of_day) +print(sim.current_datetime) + +sim.run(5) + +print(sim.time_of_day) +print(sim.current_datetime) diff --git a/quickstart/20-enable-sensors.py b/quickstart/20-enable-sensors.py index 6b9ae9c..12dfc64 100755 --- a/quickstart/20-enable-sensors.py +++ b/quickstart/20-enable-sensors.py @@ -1,40 +1,48 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -import time -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #20: Enabling and disabling sensors") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[1] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -sensors = a.get_sensors() +sensors = ego.get_sensors() # Sensors have an enabled/disabled state that can be set # By default all sensors are enabled # Disabling sensor will prevent it to send or receive messages to ROS or Cyber bridges +lidarAvailable = False for s in sensors: - print(type(s), s.enabled) + print(type(s), s.enabled) + if isinstance(s, lgsvl.LidarSensor): + lidarAvailable = True -input("Press Enter to disable lidar") +if lidarAvailable: + input("Press Enter to disable LiDAR") -for s in sensors: - if isinstance(s, lgsvl.LidarSensor): - s.enabled = False + for s in sensors: + if isinstance(s, lgsvl.LidarSensor): + s.enabled = False -for s in sensors: - print(type(s), s.enabled) + for s in sensors: + print(type(s), s.enabled) +else: + print("LiDAR sensor is not included in the selected sensor configuration. Please check the \"ego_lincoln2017mkz_apollo5\" sensor configuration in the default assets.") + input("Press Enter to stop the simulation") diff --git a/quickstart/21-map-coordinates.py b/quickstart/21-map-coordinates.py index e9a3cfe..e446266 100755 --- a/quickstart/21-map-coordinates.py +++ b/quickstart/21-map-coordinates.py @@ -1,20 +1,21 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import random -import time -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #21: Converting the map coordinates") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() @@ -28,8 +29,13 @@ # This function can take either lat/long or northing/easting pairs as inputs and will provide equivalent position and rotation vectors # Altitude and orientation are optional -t1 = sim.map_from_gps(northing = gps.northing, easting = gps.easting, altitude = gps.altitude, orientation = gps.orientation) +t1 = sim.map_from_gps( + northing=gps.northing, + easting=gps.easting, + altitude=gps.altitude, + orientation=gps.orientation, +) print("Transform from northing/easting: {}".format(t1)) -t2 = sim.map_from_gps(latitude = gps.latitude, longitude = gps.longitude) +t2 = sim.map_from_gps(latitude=gps.latitude, longitude=gps.longitude) print("Transform from lat/long without altitude/orientation: {}".format(t2)) diff --git a/quickstart/22-connecting-bridge.py b/quickstart/22-connecting-bridge.py index 0afe53d..0cdc2ef 100755 --- a/quickstart/22-connecting-bridge.py +++ b/quickstart/22-connecting-bridge.py @@ -1,36 +1,41 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl -import random import time +from environs import Env +import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #22: Connecting to a bridge") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() state = lgsvl.AgentState() state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # An EGO will not connect to a bridge unless commanded to -print("Bridge connected:", a.bridge_connected) +print("Bridge connected:", ego.bridge_connected) # The EGO is now looking for a bridge at the specified IP and port -a.connect_bridge("10.195.248.183", 9090) +ego.connect_bridge(env.str("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host), env.int("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) print("Waiting for connection...") -while not a.bridge_connected: - time.sleep(1) +while not ego.bridge_connected: + time.sleep(1) + +print("Bridge connected:", ego.bridge_connected) +print("Running the simulation for 30 seconds") -print("Bridge connected:", a.bridge_connected) +sim.run(30) diff --git a/quickstart/23-npc-callbacks.py b/quickstart/23-npc-callbacks.py index 92ce5d3..03c362d 100755 --- a/quickstart/23-npc-callbacks.py +++ b/quickstart/23-npc-callbacks.py @@ -1,20 +1,23 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import math import random +from environs import Env +import lgsvl + +print("Python API Quickstart #23: Handling NPCs callbacks") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() forward = lgsvl.utils.transform_to_forward(spawns[0]) @@ -22,33 +25,41 @@ state = lgsvl.AgentState() state.transform = sim.map_point_on_lane(spawns[0].position + 200 * forward) -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) mindist = 10.0 maxdist = 40.0 random.seed(0) + # Along with collisions and waypoints, NPCs can send a callback when they change lanes and reach a stopline def on_stop_line(agent): - print(agent.name, "reached stop line") + print(agent.name, "reached stop line") + # This will be called when an NPC begins to change lanes def on_lane_change(agent): - print(agent.name, "is changing lanes") + print(agent.name, "is changing lanes") + # This creates 4 NPCs randomly in an area around the EGO for name in ["Sedan", "SUV", "Jeep", "Hatchback"]: - angle = random.uniform(0.0, 2*math.pi) - dist = random.uniform(mindist, maxdist) + angle = random.uniform(0.0, 2 * math.pi) + dist = random.uniform(mindist, maxdist) - point = spawns[0].position + dist * math.sin(angle) * right + (225 + dist * math.cos(angle)) * forward + point = ( + spawns[0].position + + dist * math.sin(angle) * right + + (225 + dist * math.cos(angle)) * forward + ) - state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(point) - n = sim.add_agent(name, lgsvl.AgentType.NPC, state) - n.follow_closest_lane(True, 10) - n.on_lane_change(on_lane_change) - n.on_stop_line(on_stop_line) + state = lgsvl.AgentState() + state.transform = sim.map_point_on_lane(point) + npc = sim.add_agent(name, lgsvl.AgentType.NPC, state) + npc.follow_closest_lane(True, 10) + npc.on_lane_change(on_lane_change) + npc.on_stop_line(on_stop_line) -sim.run() +print("Running the simulation for 30 seconds.") +sim.run(30) diff --git a/quickstart/24-ego-drive-straight-non-realtime.py b/quickstart/24-ego-drive-straight-non-realtime.py index 272fe39..175a143 100755 --- a/quickstart/24-ego-drive-straight-non-realtime.py +++ b/quickstart/24-ego-drive-straight-non-realtime.py @@ -1,19 +1,22 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os -import lgsvl import time +from environs import Env +import lgsvl + +print("Python API Quickstart #24: Changing the Simulator timescale") +env = Env() -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() @@ -22,7 +25,7 @@ forward = lgsvl.utils.transform_to_forward(spawns[0]) # Agents can be spawned with a velocity. Default is to spawn with 0 velocity state.velocity = 20 * forward -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) print("Real time elapsed =", 0) print("Simulation time =", sim.current_time) @@ -32,7 +35,7 @@ # The simulator can be run for a set amount of time. time_limit is optional and if omitted or set to 0, then the simulator will run indefinitely t0 = time.time() -sim.run(time_limit = 4, time_scale = 2) +sim.run(time_limit=4, time_scale=2) t1 = time.time() print("Real time elapsed =", t1 - t0) print("Simulation time =", sim.current_time) @@ -44,7 +47,7 @@ input("Press Enter to continue driving for 2 more seconds (0.5x)") t2 = time.time() -sim.run(time_limit = 2, time_scale = 0.5) +sim.run(time_limit=2, time_scale=0.5) t3 = time.time() print("Real time elapsed =", t3 - t2) diff --git a/quickstart/25-waypoint-flying-npc.py b/quickstart/25-waypoint-flying-npc.py index 8005e6d..903feef 100755 --- a/quickstart/25-waypoint-flying-npc.py +++ b/quickstart/25-waypoint-flying-npc.py @@ -1,75 +1,81 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import math -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #25: NPC flying to waypoints") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() # EGO - state = lgsvl.AgentState() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) up = lgsvl.utils.transform_to_up(spawns[0]) state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -# NPC, 10 meters ahead +# NPC state = lgsvl.AgentState() state.transform.position = spawns[0].position + 10 * forward state.transform.rotation = spawns[0].rotation npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) - # This block creates the list of waypoints that the NPC will follow # Each waypoint consists of a position vector, speed, and an angular orientation vector waypoints = [] -x_max = 2 -y_max = 2 z_delta = 12 layer_mask = 0 -layer_mask |= 1 << 0 # 0 is the layer for the road (default) +layer_mask |= 1 << 0 # 0 is the layer for the road (default) px = 0 py = 0.5 for i in range(75): - speed = 6 - px = 0 - py = ((i + 1) * 0.05) if (i < 50) else (50 * 0.05 + (50 - i) * 0.05) - pz = i * z_delta * 0.05 + speed = 6 + px = 0 + py = ((i + 1) * 0.05) if (i < 50) else (50 * 0.05 + (50 - i) * 0.05) + pz = i * z_delta * 0.05 - angle = lgsvl.Vector(i, 0, 3*i) + angle = lgsvl.Vector(i, 0, 3 * i) - # Raycast the points onto the ground because BorregasAve is not flat - hit = sim.raycast(spawns[0].position + px * right + pz * forward, lgsvl.Vector(0,-1,0), layer_mask) + # Raycast the points onto the ground because BorregasAve is not flat + hit = sim.raycast( + spawns[0].position + px * right + pz * forward, + lgsvl.Vector(0, -1, 0), + layer_mask, + ) + + wp = lgsvl.DriveWaypoint( + spawns[0].position + px * right + py * up + pz * forward, speed, angle, 0, 0 + ) + waypoints.append(wp) - wp = lgsvl.DriveWaypoint(spawns[0].position + px * right + py * up + pz * forward, speed, angle, 0, 0) - waypoints.append(wp) # When the NPC is within 1m of the waypoint, this will be called def on_waypoint(agent, index): - print("waypoint {} reached".format(index)) + print("waypoint {} reached".format(index)) + # The above function needs to be added to the list of callbacks for the NPC npc.on_waypoint_reached(on_waypoint) -# The NPC needs to be given the list of waypoints. +# The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) npc.follow(waypoints) -input("Press Enter to run") +input("Press Enter to run the simulation for 12 seconds") -sim.run() +sim.run(12) diff --git a/quickstart/26-npc-trigger-waypoints.py b/quickstart/26-npc-trigger-waypoints.py index 410fd72..4dff68f 100755 --- a/quickstart/26-npc-trigger-waypoints.py +++ b/quickstart/26-npc-trigger-waypoints.py @@ -1,86 +1,97 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -import math -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) -if sim.current_scene == "BorregasAve": - sim.reset() +print("Python API Quickstart #26: NPC triggering the waypoints callbacks") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load("BorregasAve") + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) spawns = sim.get_spawn() # EGO - state = lgsvl.AgentState() forward = lgsvl.utils.transform_to_forward(spawns[0]) right = lgsvl.utils.transform_to_right(spawns[0]) state.transform = spawns[0] -a = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +state.velocity = 12 * forward +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -# NPC, 10 meters ahead +# NPC state = lgsvl.AgentState() state.transform.position = spawns[0].position + 10 * forward state.transform.rotation = spawns[0].rotation npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) vehicles = { - a: "EGO", - npc: "Sedan", + ego: "EGO", + npc: "Sedan", } + # Executed upon receiving collision callback -- NPC is expected to drive through colliding objects def on_collision(agent1, agent2, contact): - name1 = vehicles[agent1] - name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" - print("{} collided with {}".format(name1, name2)) + name1 = vehicles[agent1] + name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {}".format(name1, name2)) -a.on_collision(on_collision) + +ego.on_collision(on_collision) npc.on_collision(on_collision) # This block creates the list of waypoints that the NPC will follow # Each waypoint is an position vector paired with the speed that the NPC will drive to it waypoints = [] -x_max = 2 z_delta = 12 layer_mask = 0 -layer_mask |= 1 << 0 # 0 is the layer for the road (default) +layer_mask |= 1 << 0 # 0 is the layer for the road (default) for i in range(20): - speed = 24# if i % 2 == 0 else 12 - px = 0 - pz = (i + 1) * z_delta - # Waypoint angles are input as Euler angles (roll, pitch, yaw) - angle = spawns[0].rotation - # Raycast the points onto the ground because BorregasAve is not flat - hit = sim.raycast(spawns[0].position + px * right + pz * forward, lgsvl.Vector(0,-1,0), layer_mask) - - # Trigger is set to 10 meters for every other waypoint (0 means no trigger) - tr = 0 - if (i % 2): - tr = 10 - wp = lgsvl.DriveWaypoint(position=hit.point, speed=speed, angle=angle, idle=0, trigger_distance=tr) - waypoints.append(wp) + speed = 24 # if i % 2 == 0 else 12 + px = 0 + pz = (i + 1) * z_delta + # Waypoint angles are input as Euler angles (roll, pitch, yaw) + angle = spawns[0].rotation + # Raycast the points onto the ground because BorregasAve is not flat + hit = sim.raycast( + spawns[0].position + px * right + pz * forward, + lgsvl.Vector(0, -1, 0), + layer_mask, + ) + + # Trigger is set to 10 meters for every other waypoint (0 means no trigger) + tr = 0 + if i % 2: + tr = 10 + wp = lgsvl.DriveWaypoint( + position=hit.point, speed=speed, angle=angle, idle=0, trigger_distance=tr + ) + waypoints.append(wp) + # When the NPC is within 0.5m of the waypoint, this will be called def on_waypoint(agent, index): - print("waypoint {} reached".format(index)) + print("waypoint {} reached, waiting for ego to get closer".format(index)) + # The above function needs to be added to the list of callbacks for the NPC npc.on_waypoint_reached(on_waypoint) -# The NPC needs to be given the list of waypoints. +# The NPC needs to be given the list of waypoints. # A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) npc.follow(waypoints) -input("Press Enter to run") +input("Press Enter to run simulation for 22 seconds") -sim.run() +sim.run(22) diff --git a/quickstart/27-control-traffic-lights.py b/quickstart/27-control-traffic-lights.py index 298e6d1..20814be 100755 --- a/quickstart/27-control-traffic-lights.py +++ b/quickstart/27-control-traffic-lights.py @@ -1,21 +1,21 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) - -scene_name = "BorregasAve" +print("Python API Quickstart #27: How to Control Traffic Light") +env = Env() -if sim.current_scene == scene_name: - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load(scene_name, 42) + sim.load(lgsvl.wise.DefaultAssets.map_borregasave, 42) spawns = sim.get_spawn() @@ -23,17 +23,15 @@ forward = lgsvl.utils.transform_to_forward(spawns[0]) state.transform = spawns[0] state.transform.position = spawns[0].position + 20 * forward -ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) - -print("Python API Quickstart #27: How to Control Traffic Light") +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) # # Get a list of controllable objects controllables = sim.get_controllables("signal") -print("\n# List of controllable objects in {} scene:".format(scene_name)) +print("\n# List of controllable objects in {} scene:".format(lgsvl.wise.DefaultAssets.map_borregasave)) for c in controllables: - print(c) + print(c) -signal = sim.get_controllable(lgsvl.Vector(19, 5, 21), "signal") +signal = sim.get_controllable(lgsvl.Vector(15.5, 4.7, -23.9), "signal") print("\n# Signal of interest:") print(signal) diff --git a/quickstart/28-control-traffic-cone.py b/quickstart/28-control-traffic-cone.py index 485873d..c0e2572 100755 --- a/quickstart/28-control-traffic-cone.py +++ b/quickstart/28-control-traffic-cone.py @@ -1,21 +1,21 @@ #!/usr/bin/env python3 # -# Copyright (c) 2020 LG Electronics, Inc. +# Copyright (c) 2020-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -import os +from environs import Env import lgsvl -sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) - -scene_name = "CubeTown" +print("Python API Quickstart #28: How to Add/Control Traffic Cone") +env = Env() -if sim.current_scene == scene_name: - sim.reset() +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() else: - sim.load(scene_name, 42) + sim.load(lgsvl.wise.DefaultAssets.map_borregasave, 42) spawns = sim.get_spawn() @@ -25,25 +25,27 @@ up = lgsvl.utils.transform_to_up(spawns[0]) state.transform = spawns[0] -ego = sim.add_agent("Lincoln2017MKZ (Apollo 5.0)", lgsvl.AgentType.EGO, state) +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) -print("Python API Quickstart #28: How to Add/Control Traffic Cone") +for i in range(10 * 3): + # Create controllables in a block + start = ( + spawns[0].position + + (5 + (1.0 * (i // 6))) * forward + - (2 + (1.0 * (i % 6))) * right + ) + end = start + 10 * forward + + state = lgsvl.ObjectState() + state.transform.position = start + state.transform.rotation = spawns[0].rotation + # Set velocity and angular_velocity + state.velocity = 10 * up + state.angular_velocity = 6.5 * right + + # add controllable + o = sim.controllable_add("TrafficCone", state) -for i in range(10*3): - # Create controllables in a block - start = spawns[0].position + (5 + (1.0 * (i//6))) * forward - (2 + (1.0 * (i % 6))) * right - end = start + 10 * forward - - state = lgsvl.ObjectState() - state.transform.position = start - state.transform.rotation = spawns[0].rotation - # Set velocity and angular_velocity - state.velocity = 10 * up - state.angular_velocity = 6.5 * right - - # add controllable - o = sim.controllable_add("TrafficCone", state) - print("\nAdded {} Traffic Cones".format(i + 1)) diff --git a/quickstart/29-add-random-agents.py b/quickstart/29-add-random-agents.py new file mode 100755 index 0000000..8fbf6df --- /dev/null +++ b/quickstart/29-add-random-agents.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2020-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from environs import Env +import lgsvl + +print("Python API Quickstart #29: Adding random agents to a simulation") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +else: + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) + +spawns = sim.get_spawn() +forward = lgsvl.utils.transform_to_forward(spawns[0]) +right = lgsvl.utils.transform_to_right(spawns[0]) + +state = lgsvl.AgentState() +state.transform.position = spawns[0].position + 40 * forward +state.transform.rotation = spawns[0].rotation + +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +sim.add_random_agents(lgsvl.AgentType.NPC) +sim.add_random_agents(lgsvl.AgentType.PEDESTRIAN) + +input("Press Enter to start the simulation for 30 seconds") + +sim.run(30) diff --git a/quickstart/30-time-to-collision-trigger.py b/quickstart/30-time-to-collision-trigger.py new file mode 100755 index 0000000..69e256f --- /dev/null +++ b/quickstart/30-time-to-collision-trigger.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2020-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from environs import Env +import lgsvl + +print("Python API Quickstart #30: Using the time to collision trigger") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +else: + sim.load(lgsvl.wise.DefaultAssets.map_borregasave, 42) + +spawns = sim.get_spawn() +layer_mask = 0 +layer_mask |= 1 << 0 # 0 is the layer for the road (default) + +# EGO +state = lgsvl.AgentState() +forward = lgsvl.utils.transform_to_forward(spawns[0]) +right = lgsvl.utils.transform_to_right(spawns[0]) +spawn_state = spawns[0] +hit = sim.raycast( + spawn_state.position + forward * 40, lgsvl.Vector(0, -1, 0), layer_mask +) +spawn_state.position = hit.point +state.transform = spawn_state +state.velocity = forward * 2 +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +# NPC +state = lgsvl.AgentState() +npc_position = lgsvl.Vector(-4, -1, -48) +hit = sim.raycast(npc_position, lgsvl.Vector(0, -1, 0), layer_mask) +npc_rotation = lgsvl.Vector(0, 16, 0) +state.transform.position = hit.point +state.transform.rotation = npc_rotation +npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) + +vehicles = { + ego: "EGO", + npc: "Sedan", +} + + +# Executed upon receiving collision callback -- NPC is expected to drive through colliding objects +def on_collision(agent1, agent2, contact): + name1 = vehicles[agent1] + name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {}".format(name1, name2)) + + +ego.on_collision(on_collision) +npc.on_collision(on_collision) + +# This block creates the list of waypoints that the NPC will follow +# Each waypoint is an position vector paired with the speed that the NPC will drive to it +waypoints = [] + +for i in range(2): + speed = 8 + # Waypoint angles are input as Euler angles (roll, pitch, yaw) + angle = npc_rotation + # Raycast the points onto the ground because BorregasAve is not flat + npc_position.x += 6 + npc_position.z += 21 + hit = sim.raycast(npc_position, lgsvl.Vector(0, -1, 0), layer_mask) + + trigger = None + if i == 0: + effector = lgsvl.TriggerEffector("TimeToCollision", {}) + trigger = lgsvl.WaypointTrigger([effector]) + wp = lgsvl.DriveWaypoint( + position=hit.point, + speed=speed, + angle=angle, + idle=0, + trigger_distance=0, + trigger=trigger, + ) + waypoints.append(wp) + + +def on_waypoint(agent, index): + print("waypoint {} reached".format(index)) + + +def agents_traversed_waypoints(): + print("All agents traversed their waypoints.") + sim.stop() + + +# The above function needs to be added to the list of callbacks for the NPC +npc.on_waypoint_reached(on_waypoint) +sim.agents_traversed_waypoints(agents_traversed_waypoints) + +# The NPC needs to be given the list of waypoints. +# A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) +npc.follow(waypoints) + +input("Press Enter to run") + +sim.run() diff --git a/quickstart/31-wait-for-distance-trigger.py b/quickstart/31-wait-for-distance-trigger.py new file mode 100755 index 0000000..89b3b40 --- /dev/null +++ b/quickstart/31-wait-for-distance-trigger.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2020-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from environs import Env +import lgsvl + +print("Python API Quickstart #31: Using the wait for distance trigger") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +else: + sim.load(lgsvl.wise.DefaultAssets.map_borregasave, 42) + +spawns = sim.get_spawn() +layer_mask = 0 +layer_mask |= 1 << 0 # 0 is the layer for the road (default) + +# EGO +state = lgsvl.AgentState() +forward = lgsvl.utils.transform_to_forward(spawns[0]) +right = lgsvl.utils.transform_to_right(spawns[0]) +spawn_state = spawns[0] +hit = sim.raycast( + spawn_state.position + forward * 40, lgsvl.Vector(0, -1, 0), layer_mask +) +spawn_state.position = hit.point +state.transform = spawn_state +state.velocity = forward * 2 +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +# NPC +state = lgsvl.AgentState() +npc_position = lgsvl.Vector(-4, -1, -48) +hit = sim.raycast(npc_position, lgsvl.Vector(0, -1, 0), layer_mask) +npc_rotation = lgsvl.Vector(0, 16, 0) +state.transform.position = hit.point +state.transform.rotation = npc_rotation +npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) + +vehicles = { + ego: "EGO", + npc: "Sedan", +} + + +# Executed upon receiving collision callback -- NPC is expected to drive through colliding objects +def on_collision(agent1, agent2, contact): + name1 = vehicles[agent1] + name2 = vehicles[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {}".format(name1, name2)) + + +ego.on_collision(on_collision) +npc.on_collision(on_collision) + +# This block creates the list of waypoints that the NPC will follow +# Each waypoint is an position vector paired with the speed that the NPC will drive to it +waypoints = [] + +for i in range(2): + speed = 8 # if i % 2 == 0 else 12 + # Waypoint angles are input as Euler angles (roll, pitch, yaw) + angle = npc_rotation + # Raycast the points onto the ground because BorregasAve is not flat + npc_position.x += 6 + npc_position.z += 21 + hit = sim.raycast(npc_position, lgsvl.Vector(0, -1, 0), layer_mask) + + trigger = None + if i == 0: + parameters = {"maxDistance": 4.0} + effector = lgsvl.TriggerEffector("WaitForDistance", parameters) + trigger = lgsvl.WaypointTrigger([effector]) + wp = lgsvl.DriveWaypoint( + position=hit.point, + speed=speed, + angle=angle, + idle=0, + trigger_distance=0, + trigger=trigger, + ) + waypoints.append(wp) + + +def on_waypoint(agent, index): + print("waypoint {} reached".format(index)) + + +def agents_traversed_waypoints(): + print("All agents traversed their waypoints.") + sim.stop() + + +# The above function needs to be added to the list of callbacks for the NPC +npc.on_waypoint_reached(on_waypoint) +sim.agents_traversed_waypoints(agents_traversed_waypoints) + +# The NPC needs to be given the list of waypoints. +# A bool can be passed as the 2nd argument that controls whether or not the NPC loops over the waypoints (default false) +npc.follow(waypoints) + +input("Press Enter to run") + +sim.run() diff --git a/quickstart/32-pedestrian-time-to-collision.py b/quickstart/32-pedestrian-time-to-collision.py new file mode 100755 index 0000000..ac89a07 --- /dev/null +++ b/quickstart/32-pedestrian-time-to-collision.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from environs import Env +import lgsvl + +print("Python API Quickstart #1: Using the time to collision trigger with a pedestrian") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +else: + sim.load(lgsvl.wise.DefaultAssets.map_borregasave, 42) + +spawns = sim.get_spawn() +layer_mask = 0 +layer_mask |= 1 << 0 # 0 is the layer for the road (default) + +# EGO +state = lgsvl.AgentState() +ego_position = lgsvl.Vector(342.0, 0.0, -87.7) +hit = sim.raycast(ego_position, lgsvl.Vector(0, -1, 0), layer_mask) +state.transform.position = hit.point +state.transform.rotation = lgsvl.Vector(0.0, 15.0, 0.0) +forward = lgsvl.Vector(0.2, 0.0, 0.8) +state.velocity = forward * 15 +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +# Pedestrian +state = lgsvl.AgentState() +pedestrian_position = lgsvl.Vector(350.0, 0.0, -12) +hit = sim.raycast(pedestrian_position, lgsvl.Vector(0, -1, 0), layer_mask) +pedestrian_rotation = lgsvl.Vector(0.0, 105.0, 0.0) +state.transform.position = hit.point +state.transform.rotation = pedestrian_rotation +pedestrian = sim.add_agent("Bob", lgsvl.AgentType.PEDESTRIAN, state) + +agents = {ego: "EGO", pedestrian: "Bob"} + + +# Executed upon receiving collision callback -- pedestrian is expected to walk into colliding objects +def on_collision(agent1, agent2, contact): + name1 = agents[agent1] + name2 = agents[agent2] if agent2 is not None else "OBSTACLE" + print("{} collided with {}".format(name1, name2)) + + +ego.on_collision(on_collision) +pedestrian.on_collision(on_collision) + +# This block creates the list of waypoints that the pedestrian will follow +# Each waypoint is an position vector paired with the speed that the pedestrian will walk to +waypoints = [] + +trigger = None +speed = 3 +hit = sim.raycast( + pedestrian_position + lgsvl.Vector(7.4, 0.0, -2.2), + lgsvl.Vector(0, -1, 0), + layer_mask, +) +effector = lgsvl.TriggerEffector("TimeToCollision", {}) +trigger = lgsvl.WaypointTrigger([effector]) +wp = lgsvl.WalkWaypoint( + position=hit.point, speed=speed, idle=0, trigger_distance=0, trigger=trigger +) +waypoints.append(wp) + +hit = sim.raycast( + pedestrian_position + lgsvl.Vector(12.4, 0.0, -3.4), + lgsvl.Vector(0, -1, 0), + layer_mask, +) +wp = lgsvl.WalkWaypoint( + position=hit.point, speed=speed, idle=0, trigger_distance=0, trigger=None +) +waypoints.append(wp) + + +def on_waypoint(agent, index): + print("waypoint {} reached".format(index)) + + +def agents_traversed_waypoints(): + print("All agents traversed their waypoints.") + sim.stop() + + +# The above function needs to be added to the list of callbacks for the pedestrian +pedestrian.on_waypoint_reached(on_waypoint) +sim.agents_traversed_waypoints(agents_traversed_waypoints) + +# The pedestrian needs to be given the list of waypoints. A bool can be passed as the 2nd argument that controls +# whether or not the pedestrian loops over the waypoints (default false) +pedestrian.follow(waypoints, False) + +input("Press Enter to run") + +sim.run() diff --git a/quickstart/33-ego-drive-stepped.py b/quickstart/33-ego-drive-stepped.py new file mode 100755 index 0000000..22b130d --- /dev/null +++ b/quickstart/33-ego-drive-stepped.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2020-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import os +import lgsvl +import time + +print("Python API Quickstart #33: Stepping a simulation") +sim = lgsvl.Simulator(os.environ.get("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), lgsvl.wise.SimulatorSettings.simulator_port) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +# Re-load scene and set random seed +sim.load(lgsvl.wise.DefaultAssets.map_borregasave, seed=123) + +spawns = sim.get_spawn() + +state = lgsvl.AgentState() +state.transform = spawns[0] +forward = lgsvl.utils.transform_to_forward(spawns[0]) + +# Agents can be spawned with a velocity. Default is to spawn with 0 velocity +# state.velocity = 20 * forward + +# In order for Apollo to drive with stepped simulation we must add the clock sensor +# and we must also set clock_mode to MODE_MOCK in Apollo's cyber/conf/cyber.pb.conf +# Note that MODE_MOCK is only supported in Apollo master (6.0 or later)! +# Refer to https://www.svlsimulator.com/docs/sensor-json-options/#clock + +# We can test Apollo with standard MKZ or MKZ with ground truth sensors +# Refer to https://www.svlsimulator.com/docs/modular-testing/ +ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5, lgsvl.AgentType.EGO, state) + +# An EGO will not connect to a bridge unless commanded to +print("Bridge connected:", ego.bridge_connected) + +# The EGO looks for a (Cyber) bridge at the specified IP and port +ego.connect_bridge(lgsvl.wise.SimulatorSettings.bridge_host, lgsvl.wise.SimulatorSettings.bridge_port) +# uncomment to wait for bridge connection; script will drive ego if bridge not found +# print("Waiting for connection...") +# while not ego.bridge_connected: +time.sleep(1) + +print("Bridge connected:", ego.bridge_connected) + +# spawn random NPCs +sim.add_random_agents(lgsvl.AgentType.NPC) +sim.add_random_agents(lgsvl.AgentType.PEDESTRIAN) + +t0 = time.time() +s0 = sim.current_time +print() +print("Total real time elapsed = {:5.3f}".format(0)) +print("Simulation time = {:5.1f}".format(s0)) +print("Simulation frames =", sim.current_frame) + +# let simulator initialize and settle a bit before starting +sim.run(time_limit=2) + +t1 = time.time() +s1 = sim.current_time +print() +print("Total real time elapsed = {:5.3f}".format(t1 - t0)) +print("Simulation settle time = {:5.1f}".format(s1 - s0)) +print("Simulation settle frames =", sim.current_frame) + +# tell ego to drive forward, or not (if apollo is driving) +if not ego.bridge_connected: + state = ego.state + forward = lgsvl.utils.transform_to_forward(state.transform) + state.velocity = 20 * forward + ego.state = state + duration = 15 +else: + duration = 45 + +step_time = 0.10 + +step_rate = int(1.0 / step_time) +steps = duration * step_rate +print() +print("Stepping forward for {} steps of {}s per step" .format(steps, step_time)) +input("Press Enter to start:") + +# The simulator can be run for a set amount of time. +# time_limit is optional and if omitted or set to 0, then the simulator will run indefinitely +t0 = time.time() +for i in range(steps): + t1 = time.time() + sim.run(time_limit=step_time) + t2 = time.time() + s2 = sim.current_time + + state = ego.state + pos = state.position + speed = state.speed * 3.6 + + # if Apollo is not driving + if not ego.bridge_connected: + state.velocity = 20 * forward + ego.state = state + + print("Sim time = {:5.2f}".format(s2 - s1) + "; Real time elapsed = {:5.3f}; ".format(t2 - t1), end='') + print("Speed = {:4.1f}; Position = {:5.3f},{:5.3f},{:5.3f}".format(speed, pos.x, pos.y, pos.z)) + time.sleep(0.2) + +t3 = time.time() + +# Final statistics +print("Total real time elapsed = {:5.3f}".format(t3 - t0)) +print("Simulation time = {:5.1f}".format(sim.current_time)) +print("Simulation frames =", sim.current_frame) diff --git a/quickstart/34-simulator-cam-set.py b/quickstart/34-simulator-cam-set.py new file mode 100755 index 0000000..8f84e95 --- /dev/null +++ b/quickstart/34-simulator-cam-set.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +from environs import Env +import lgsvl + +print("Python API Quickstart #34: Setting the fixed camera position") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) +if sim.current_scene == lgsvl.wise.DefaultAssets.map_borregasave: + sim.reset() +else: + sim.load(lgsvl.wise.DefaultAssets.map_borregasave) + +# This creates a transform in Unity world coordinates +tr = lgsvl.Transform(lgsvl.Vector(10, 50, 0), lgsvl.Vector(90, 0, 0)) + +spawns = sim.get_spawn() + +state = lgsvl.AgentState() +state.transform = spawns[0] +forward = lgsvl.utils.transform_to_forward(spawns[0]) +state.velocity = 20 * forward +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +# This function sets the camera to free state and applies the transform to the camera rig +sim.set_sim_camera(tr) +print("Set transform: {}".format(tr)) + +input("Press Enter to drive forward for 4 seconds") +sim.run(time_limit=4.0) diff --git a/quickstart/35-spawn-multi-robots.py b/quickstart/35-spawn-multi-robots.py new file mode 100755 index 0000000..f4a9727 --- /dev/null +++ b/quickstart/35-spawn-multi-robots.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import time +from environs import Env +import lgsvl + +print("Python API Quickstart #35: Spawning multi robots with bridge connections") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) + +map_seocho = lgsvl.wise.DefaultAssets.map_lgseocho +robots = [ + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot1, + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot2, + lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2_multi_robot3, +] + +if sim.current_scene == map_seocho: + sim.reset() +else: + sim.load(map_seocho) + +spawns = sim.get_spawn() +spawn = spawns[1] +right = lgsvl.utils.transform_to_right(spawn) + +for i, robot in enumerate(robots): + state = lgsvl.AgentState() + state.transform.position = spawn.position + (1.0 * i * right) + state.transform.rotation = spawn.rotation + ego = sim.add_agent(robot, lgsvl.AgentType.EGO, state) + print("Spawned a robot at:", state.transform.position) + + ego.connect_bridge(env.str("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host), env.int("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + print("Waiting for connection...") + while not ego.bridge_connected: + time.sleep(1) + + print("Bridge connected:", ego.bridge_connected) + +print("Running the simulation...") +sim.run() diff --git a/quickstart/36-send-destination-to-nav2.py b/quickstart/36-send-destination-to-nav2.py new file mode 100755 index 0000000..7c4362d --- /dev/null +++ b/quickstart/36-send-destination-to-nav2.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import lgsvl +from environs import Env +from lgsvl import Vector, Quaternion + +print("Python API Quickstart #36: Sending destinations to Navigation2") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) + +map_seocho = lgsvl.wise.DefaultAssets.map_lgseocho +ego_cloi = lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2 + +if sim.current_scene == map_seocho: + sim.reset() +else: + sim.load(map_seocho) + +sim.set_time_of_day(0, fixed=True) +spawns = sim.get_spawn() +state = lgsvl.AgentState() +state.transform = spawns[0] + +ego = sim.add_agent(env.str("LGSVL__VEHICLE_0", ego_cloi), lgsvl.AgentType.EGO, state) +ego.connect_bridge(env.str("LGSVL__AUTOPILOT_0_HOST", lgsvl.wise.SimulatorSettings.bridge_host), env.int("LGSVL__AUTOPILOT_0_PORT", lgsvl.wise.SimulatorSettings.bridge_port)) + +i = 0 + +# List of destinations in Nav2 costmap coordinates +ros_destinations = [ + ((3.118, -0.016, 0), (0, 0, 0.0004, 0.999)), + ((3.126, -10.025, 0), (0, 0, 0.006, 0.999)), + ((11.171, -9.875, 0), (0, 0, 0.707, 0.707)), + ((11.882, -0.057, 0), (0, 0, 0.699, 0.715)), + ((12.351, 11.820, 0), (0, 0, -0.999, 0.0004)), + ((2.965, 11.190, 0), (0, 0, -0.707, 0.707)), + ((2.687, -0.399, 0), (0, 0, -0.0036, 0.999)), +] + +ego.set_initial_pose() +sim.run(5) + + +def send_next_destination(agent): + global i + print(f"{i}: {agent.name} reached destination") + i += 1 + if i >= len(ros_destinations): + print("Iterated all destinations successfully. Stopping...") + sim.stop() + return + + ros_dst = ros_destinations[i] + next_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1])) + ego.set_destination(next_dst) + + +ros_dst = ros_destinations[0] +first_dst = sim.map_from_nav(Vector(*ros_dst[0]), Quaternion(*ros_dst[1])) +ego.set_destination(first_dst) +ego.on_destination_reached(send_next_destination) + +sim.run() + +print("Done!") diff --git a/quickstart/98-npc-behaviour.py b/quickstart/98-npc-behaviour.py new file mode 100755 index 0000000..24a846a --- /dev/null +++ b/quickstart/98-npc-behaviour.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2019-2021 LG Electronics, Inc. +# +# This software contains code licensed as described in LICENSE. +# + +import math +import random +from environs import Env +import lgsvl + +print("Python API Quickstart #98: Simulating different NPCs behaviours") +env = Env() + +sim = lgsvl.Simulator(env.str("LGSVL__SIMULATOR_HOST", lgsvl.wise.SimulatorSettings.simulator_host), env.int("LGSVL__SIMULATOR_PORT", lgsvl.wise.SimulatorSettings.simulator_port)) + +drunkDriverAvailable = False +trailerAvailable = 0 + +print("Current Scene = {}".format(sim.current_scene)) +# Loads the named map in the connected simulator. +if sim.current_scene == lgsvl.wise.DefaultAssets.map_cubetown: + sim.reset() +else: + print("Loading Scene = {}".format(lgsvl.wise.DefaultAssets.map_cubetown)) + sim.load(lgsvl.wise.DefaultAssets.map_cubetown) + +agents = sim.available_agents +print("agents:") +for i in range(len(agents)): + agent = agents[i] + if agent["name"] == "TrailerTruckTest": + trailerAvailable |= 1 + if agent["name"] == "MackAnthemStandupSleeperCab2018": + trailerAvailable |= 2 + print( + "#{:>2}: {} {:40} ({:9}) {}".format( + i, + ("✔️" if agent["loaded"] else "❌"), + agent["name"], + agent["NPCType"], + agent["AssetGuid"], + ) + ) + +behaviours = sim.available_npc_behaviours + + +print("behaviours:") +for i in range(len(behaviours)): + if behaviours[i]["name"] == "NPCDrunkDriverBehaviour": + drunkDriverAvailable = True + if behaviours[i]["name"] == "NPCTrailerBehaviour": + trailerAvailable |= 4 + print("#{:>2}: {}".format(i, behaviours[i]["name"])) + +spawns = sim.get_spawn() + +state = lgsvl.AgentState() +state.transform = spawns[0] +sim.add_agent(env.str("LGSVL__VEHICLE_0", lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5), lgsvl.AgentType.EGO, state) + +mindist = 10.0 +maxdist = 40.0 + +random.seed(0) +trailerAvailable = trailerAvailable == (1 | 2 | 4) + +print( + "Trailer support: {} Drunk Driver support: {}".format( + "✔️" if trailerAvailable else "❌", "✔️" if drunkDriverAvailable else "❌" + ) +) + +while True: + if trailerAvailable: + inp = input( + "Enter index of Agent to spawn, t to spawn truck and trailer, r to run:" + ) + else: + inp = input("Enter index of Agent to spawn, r to run:") + + angle = random.uniform(0.0, 2 * math.pi) + dist = random.uniform(mindist, maxdist) + spawn = random.choice(spawns) + sx = spawn.position.x + sy = spawn.position.y + sz = spawn.position.z + point = lgsvl.Vector(sx + dist * math.cos(angle), sy, sz + dist * math.sin(angle)) + state = lgsvl.AgentState() + state.transform = sim.map_point_on_lane(point) + + if inp == "r": + print("running.") + sim.run() + + if inp == "t" and trailerAvailable: + truck = sim.add_agent( + "MackAnthemStandupSleeperCab2018", lgsvl.AgentType.NPC, state + ) + truck.follow_closest_lane(True, 5.6) + trailer = sim.add_agent("TrailerTruckTest", lgsvl.AgentType.NPC, state) + trailer.set_behaviour("NPCTrailerBehaviour") + sim.remote.command( + "agent/trailer/attach", {"trailer_uid": trailer.uid, "truck_uid": truck.uid} + ) + + else: + try: + agentIndex = int(inp) + if agentIndex > len(agents): + print("index out of range") + continue + + agent = agents[agentIndex]["name"] + print("Selected {}".format(agent)) + npc = sim.add_agent(agent, lgsvl.AgentType.NPC, state) + + if drunkDriverAvailable: + inp = input("make drunk driver? yN") + if inp == "y" or inp == "Y": + npc.set_behaviour("NPCDrunkDriverBehaviour") + npc.remote.command( + "agent/drunk/config", + { + "uid": npc.uid, + "correctionMinTime": 0.0, + "correctionMaxTime": 0.6, + "steerDriftMin": 0.00, + "steerDriftMax": 0.09, + }, + ) + + npc.follow_closest_lane(True, 5.6) + print("spawned agent {}, uid {}".format(agent, npc.uid)) + + except ValueError: + print("expected a number") diff --git a/quickstart/99-utils-examples.py b/quickstart/99-utils-examples.py index 3f968c4..7002f92 100755 --- a/quickstart/99-utils-examples.py +++ b/quickstart/99-utils-examples.py @@ -1,13 +1,19 @@ #!/usr/bin/env python3 # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # -from lgsvl import * -from lgsvl.utils import * +from lgsvl import Transform, Vector +from lgsvl.utils import ( + transform_to_matrix, + matrix_inverse, + matrix_multiply, + vector_multiply, +) +print("Python API Quickstart #99: Using the LGSVL utilities") # global "world" transform (for example, car position) world = Transform(Vector(10, 20, 30), Vector(11, 22, 33)) diff --git a/requirements.txt b/requirements.txt index 5a2721d..ec2d6d4 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,15 @@ -websockets>=7.0 + +# - PACKAGE >= PKG_VERSION ; python_version >= PYTHON_VERSION +# PKG_VERSION is the first version of PACKAGE that supports PYTHON_VERSION +# - PACKAGE < PKG_VERSION ; python_version < PYTHON_VERSION +# PKG_VERSION is the first version of PACKAGE that no longer supports +# PYTHON_VERSION; add entries for PYTHON_VERSION > "python_requires" of +# lgsvl in setup.py +# - Keep in alphabetical order. +environs >= 8.1.0 ; python_version >= '3.9' +numpy < 1.20 ; python_version < '3.7' +numpy >= 1.20 ; python_version >= '3.7' +websocket-client >= 0.58.0 ; python_version >= '3.8' +websockets < 8.1 ; python_version < '3.6.1' +websockets >= 8.1 ; python_version >= '3.8' +websockets >= 9.0.1 ; python_version >= '3.9' diff --git a/resource/lgsvl b/resource/lgsvl new file mode 100644 index 0000000..e69de29 diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..34a94f7 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lgsvl +[install] +install_scripts=$base/lib/lgsvl diff --git a/setup.py b/setup.py index 8731ccc..3719c6b 100755 --- a/setup.py +++ b/setup.py @@ -2,19 +2,118 @@ from setuptools import setup +# Without enabling the user site, "python3 -m pip install --user -e ." fails +# with: +# +# WARNING: The user site-packages directory is disabled. +# error: can't create or remove files in install directory +# ... +# [Errno 13] Permission denied: '/usr/local/lib/python3.6/dist-packages/' +# +# Taken from https://github.com/pypa/pip/issues/7953#issuecomment-645133255. +import site +import sys +site.ENABLE_USER_SITE = "--user" in sys.argv[1:] + + +def get_version(git_tag): + """ + Returns a PEP 440 version identifier + + [+] + + derived from its parameter and the output from: + + git describe --match --tags + + which has the format: + + [--g] + + The public version identifier is formed from . PEP 440 versioning + allows only digits without leading zeros in the release segments of public + version identifiers, so they are dropped along with any "P"-s found in + . And PEP 440 does not allow hyphens in pre-release segments, so + any "-rc"-s are replaced with "rc". Fortunately, this mapping doesn't + introduce any ambiguities when used for the tags that currently exist. + + The optional local version label is formed from the portion of the + "git describe" output which follows with all hyphens converted to + periods (except the first, which is dropped). This segment will be empty if: + - Git is not available, or + - the working tree is not a Git repository, or + - the tag is not present or not on the currrent branch, or + - the HEAD of the current branch is coincident with the tag . + + NB. - Not using https://pypi.org/project/setuptools-git-version/ because it + passes "--long" to "git describe". + - Not using https://pypi.org/project/setuptools-scm/ because it's way + more complicated to configure it to do what is wanted. + """ + + # ASSERT( has no more than one leading "0", no more than one "-rc", + # and no more than one "P", which is expected to be at the end -- but + # this is not checked) + public_version_identifier = git_tag.replace(".0", ".", 1) \ + .replace("P", "", 1) \ + .replace("-rc", "rc", 1) + from os import getenv + try: + from git import Repo + # PWD is the directory from where we invoked "pip install", which is + # the root of the Git repository; when setup() is + # called, pip has changed the current directory to be under /tmp. + repo = Repo(path=getenv('PWD')) + val = repo.git.describe('--match', git_tag, '--tags') + except Exception: + val = "" + + # Remove and convert the first hyphen to a "+": + local_version_label_with_plus = val.replace(git_tag, '', 1) \ + .replace('-', '+', 1) \ + .replace('-', '.') + + return public_version_identifier + local_version_label_with_plus + + +package_name = 'lgsvl' + setup( - name="lgsvl", - description="LGSVL Simulator Api", + name=package_name, + version=get_version('2021.1'), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + description="Python API for SVL Simulator", author="LGSVL", - author_email="contact@lgsvlsimulator.com", - python_requires=">=3.5.0", - url="https://github.com/lgsvl/simulator", - packages=["lgsvl"], - install_requires=["websockets==7.0"], - license="Other", + author_email="contact@svlsimulator.com", + python_requires=">=3.6.0", + url="https://github.com/lgsvl/PythonAPI", + packages=["lgsvl", "lgsvl.dreamview", "lgsvl.evaluator", "lgsvl.wise"], + install_requires=[ + "environs", + "numpy", + "setuptools", + "websocket-client", + "websockets" + ], + setup_requires=[ + "GitPython==3.1.14" + ], + extras_require={ + "ci": [ + "flake8>=3.7.0" + ], + }, + zip_safe=True, + maintainer='Hadi Tabatabaee', + maintainer_email='hadi.tabatabaee@lge.com', + license='Other', classifiers=[ "License :: Other/Proprietary License", "Programming Language :: Python :: 3", - "Programming Language :: Python :: 3.5", + "Programming Language :: Python :: 3.6", ], ) diff --git a/tests/__init__.py b/tests/__init__.py index 9fba5bb..49d8b69 100644 --- a/tests/__init__.py +++ b/tests/__init__.py @@ -13,6 +13,7 @@ from .test_peds import TestPeds from .test_utils import TestUtils + def load_tests(loader, standard_tests, pattern): suite = unittest.TestSuite() suite.addTests(loader.loadTestsFromTestCase(TestNPC)) # must be first diff --git a/tests/common.py b/tests/common.py index 33f0370..bc38f4d 100644 --- a/tests/common.py +++ b/tests/common.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -11,55 +11,62 @@ class TestTimeout(Exception): pass + class TestException(Exception): pass + class SimConnection: - def __init__(self, seconds=30, scene="BorregasAve", error_message=None, load_scene=True): - if error_message is None: - error_message = 'test timed out after {}s.'.format(seconds) - self.seconds = seconds - self.error_message = error_message - self.scene = scene - self.load_scene = load_scene - - def handle_timeout(self, signum, frame): - raise TestTimeout(self.error_message) - - def __enter__(self): - signal.signal(signal.SIGALRM, self.handle_timeout) - signal.alarm(self.seconds) - - self.sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) - if self.load_scene: - if self.sim.current_scene == self.scene: - self.sim.reset() - signal.alarm(self.seconds - 20) - else: - self.sim.load(self.scene) - return self.sim - - def __exit__(self, exc_type, exc_val, exc_tb): - signal.alarm(0) - # agents = self.sim.get_agents() - # for a in agents: - # self.sim.remove_agent(a) - self.sim.close() + def __init__(self, seconds=30, scene=lgsvl.wise.DefaultAssets.map_borregasave, error_message=None, load_scene=True): + if error_message is None: + error_message = 'test timed out after {}s.'.format(seconds) + self.seconds = seconds + self.error_message = error_message + self.scene = scene + self.load_scene = load_scene + + def handle_timeout(self, signum, frame): + raise TestTimeout(self.error_message) + + def __enter__(self): + signal.signal(signal.SIGALRM, self.handle_timeout) + signal.alarm(self.seconds) + + self.sim = lgsvl.Simulator(os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1"), 8181) + if self.load_scene: + if self.sim.current_scene == self.scene: + self.sim.reset() + signal.alarm(self.seconds - 20) + else: + self.sim.load(self.scene) + return self.sim + + def __exit__(self, exc_type, exc_val, exc_tb): + signal.alarm(0) + # agents = self.sim.get_agents() + # for a in agents: + # self.sim.remove_agent(a) + self.sim.close() + def spawnState(sim, index=0): - state = lgsvl.AgentState() - state.transform = sim.get_spawn()[index] - return state + state = lgsvl.AgentState() + state.transform = sim.get_spawn()[index] + return state + + +def cmEqual(self, a, b, msg): # Test vectors within 1cm + self.assertAlmostEqual(a.x, b.x, 2, msg) + self.assertAlmostEqual(a.y, b.y, 2, msg) + self.assertAlmostEqual(a.z, b.z, 2, msg) -def cmEqual(self, a, b, msg): # Test vectors within 1cm - self.assertAlmostEqual(a.x, b.x, 2, msg) - self.assertAlmostEqual(a.y, b.y, 2, msg) - self.assertAlmostEqual(a.z, b.z, 2, msg) -def mEqual(self, a, b, msg): # Test vectors within 1cm - self.assertAlmostEqual(a.x, b.x, delta=1.5, msg=msg) - self.assertAlmostEqual(a.y, b.y, delta=1.5, msg=msg) - self.assertAlmostEqual(a.z, b.z, delta=1.5, msg=msg) +def mEqual(self, a, b, msg): # Test vectors within 1cm + self.assertAlmostEqual(a.x, b.x, delta=1.5, msg=msg) + self.assertAlmostEqual(a.y, b.y, delta=1.5, msg=msg) + self.assertAlmostEqual(a.z, b.z, delta=1.5, msg=msg) + def notAlmostEqual(a,b): - return round(a-b,7) != 0 + return round(a-b,7) != 0 + diff --git a/tests/test_EGO.py b/tests/test_EGO.py index fbffbfb..775951e 100644 --- a/tests/test_EGO.py +++ b/tests/test_EGO.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -8,18 +8,20 @@ import math import lgsvl -from .common import SimConnection, spawnState, cmEqual, notAlmostEqual +from .common import SimConnection, spawnState, cmEqual + # TODO add tests for bridge connection + class TestEGO(unittest.TestCase): - def test_agent_name(self): # Check if EGO Apollo is created + def test_agent_name(self): # Check if EGO Apollo is created with SimConnection() as sim: agent = self.create_EGO(sim) - self.assertEqual(agent.name, "Jaguar2015XE (Apollo 3.0)") + self.assertEqual(agent.name, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) - def test_different_spawns(self): # Check if EGO is spawned in the spawn positions + def test_different_spawns(self): # Check if EGO is spawned in the spawn positions with SimConnection() as sim: spawns = sim.get_spawn() agent = self.create_EGO(sim) @@ -28,13 +30,13 @@ def test_different_spawns(self): # Check if EGO is spawned in the spawn position state = spawnState(sim) state.transform = spawns[1] - agent2 = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + agent2 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) cmEqual(self, agent2.state.position, spawns[1].position, "Spawn Position 1") cmEqual(self, agent2.state.rotation, spawns[1].rotation, "Spawn Rotation 1") - def test_agent_velocity(self): # Check EGO velocity - with SimConnection() as sim: + def test_agent_velocity(self): # Check EGO velocity + with SimConnection(60) as sim: state = spawnState(sim) agent = self.create_EGO(sim) cmEqual(self, agent.state.velocity, state.velocity, "0 Velocity") @@ -42,60 +44,60 @@ def test_agent_velocity(self): # Check EGO velocity sim.reset() right = lgsvl.utils.transform_to_right(state.transform) state.velocity = -50 * right - agent = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) - + agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) + cmEqual(self, agent.state.velocity, state.velocity, "50 Velocity") - def test_ego_different_directions(self): # Check that the xyz velocities equate to xyz changes in position - with SimConnection(40) as sim: + def test_ego_different_directions(self): # Check that the xyz velocities equate to xyz changes in position + with SimConnection(60) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) up = lgsvl.utils.transform_to_up(state.transform) right = lgsvl.utils.transform_to_right(state.transform) state.velocity = -10 * right - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sim.run(.5) target = state.position - 3 * right self.assertLess((ego.state.position - target).magnitude(), 1) sim.remove_agent(ego) - + state.velocity = 10 * up - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sim.run(.5) target = state.position + 3 * up self.assertLess((ego.state.position - target).magnitude(), 1) sim.remove_agent(ego) state.velocity = 10 * forward - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sim.run(.5) target = state.position + 4 * forward self.assertLess((ego.state.position - target).magnitude(), 1) - def test_speed(self): # check that speed returns a reasonable number + def test_speed(self): # check that speed returns a reasonable number with SimConnection() as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) up = lgsvl.utils.transform_to_up(state.transform) right = lgsvl.utils.transform_to_right(state.transform) state.velocity = -10 * right + 10 * up + 10 * forward - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) self.assertAlmostEqual(ego.state.speed, math.sqrt(300), places=3) @unittest.skip("No highway in currents maps") - def test_rotation_on_highway_ramp(self): # Check that vehicle is rotated when spawned on the highway ramp + def test_rotation_on_highway_ramp(self): # Check that vehicle is rotated when spawned on the highway ramp with SimConnection() as sim: state = spawnState(sim) - state.transform.position = lgsvl.Vector(100.4229, 15.67488, -469.6401) - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform.position = lgsvl.Vector(469.6401, 15.67488, 100.4229) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) self.assertAlmostEqual(ego.state.rotation.z, state.rotation.z) sim.run(0.5) self.assertAlmostEqual(ego.state.rotation.z, 356, delta=0.5) - def test_ego_steering(self): # Check that a steering command can be given to an EGO vehicle, and the car turns + def test_ego_steering(self): # Check that a steering command can be given to an EGO vehicle, and the car turns with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.3 control.steering = -1.0 @@ -105,9 +107,9 @@ def test_ego_steering(self): # Check that a steering command can be given to an finalRotation = ego.state.rotation self.assertNotAlmostEqual(initialRotation.y, finalRotation.y) - def test_ego_throttle(self): # Check that a throttle command can be given to an EGO vehicle, and the car accelerates + def test_ego_throttle(self): # Check that a throttle command can be given to an EGO vehicle, and the car accelerates with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.5 ego.apply_control(control, True) @@ -116,10 +118,10 @@ def test_ego_throttle(self): # Check that a throttle command can be given to an finalSpeed = ego.state.speed self.assertGreater(finalSpeed, initialSpeed) - def test_ego_braking(self): # Check that a brake command can be given to an EGO vehicle, and the car stops sooner than without brakes + def test_ego_braking(self): # Check that a brake command can be given to an EGO vehicle, and the car stops sooner than without brakes with SimConnection(60) as sim: state = spawnState(sim) - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -128,9 +130,9 @@ def test_ego_braking(self): # Check that a brake command can be given to an EGO ego.apply_control(control,True) sim.run(3) noBrakeDistance = (ego.state.position - state.position).magnitude() - + sim.reset() - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -142,10 +144,10 @@ def test_ego_braking(self): # Check that a brake command can be given to an EGO brakeDistance = (ego.state.position - state.position).magnitude() self.assertLess(brakeDistance, noBrakeDistance) - def test_ego_handbrake(self): # Check that the handbrake can be enable on an EGO vehicle, and the car stops sooner than without brakes + def test_ego_handbrake(self): # Check that the handbrake can be enable on an EGO vehicle, and the car stops sooner than without brakes with SimConnection(60) as sim: state = spawnState(sim) - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -154,9 +156,9 @@ def test_ego_handbrake(self): # Check that the handbrake can be enable on an EGO ego.apply_control(control,True) sim.run(3) noBrakeDistance = (ego.state.position - state.position).magnitude() - + sim.reset() - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -167,11 +169,11 @@ def test_ego_handbrake(self): # Check that the handbrake can be enable on an EGO sim.run(3) brakeDistance = (ego.state.position - state.position).magnitude() self.assertLess(brakeDistance, noBrakeDistance) - - def test_ego_reverse(self): # Check that the gear can be changed in an EGO vehicle, and the car moves in reverse + + def test_ego_reverse(self): # Check that the gear can be changed in an EGO vehicle, and the car moves in reverse with SimConnection() as sim: state = spawnState(sim) - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 0.5 control.reverse = True @@ -182,10 +184,10 @@ def test_ego_reverse(self): # Check that the gear can be changed in an EGO vehic diff = ego.state.position - target self.assertLess((diff).magnitude(), 1) - def test_not_sticky_control(self): # Check that the a non sticky control is removed + def test_not_sticky_control(self): # Check that the a non sticky control is removed with SimConnection(60) as sim: state = spawnState(sim) - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -193,7 +195,7 @@ def test_not_sticky_control(self): # Check that the a non sticky control is remo stickySpeed = ego.state.speed sim.reset() - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) control = lgsvl.VehicleControl() control.throttle = 1 ego.apply_control(control, True) @@ -203,9 +205,9 @@ def test_not_sticky_control(self): # Check that the a non sticky control is remo finalSpeed = ego.state.speed self.assertGreater(stickySpeed, finalSpeed) - def test_vary_throttle(self): # Check that different throttle values accelerate differently - with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + def test_vary_throttle(self): # Check that different throttle values accelerate differently + with SimConnection(40) as sim: + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.5 ego.apply_control(control, True) @@ -213,16 +215,16 @@ def test_vary_throttle(self): # Check that different throttle values accelerate initialSpeed = ego.state.speed sim.reset() - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.1 ego.apply_control(control, True) sim.run(1) self.assertLess(ego.state.speed, initialSpeed) - def test_vary_steering(self): # Check that different steering values turn the car differently - with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + def test_vary_steering(self): # Check that different steering values turn the car differently + with SimConnection(40) as sim: + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.5 control.steering = -0.8 @@ -231,7 +233,7 @@ def test_vary_steering(self): # Check that different steering values turn the ca initialAngle = ego.state.rotation.y sim.reset() - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.throttle = 0.5 control.steering = -0.3 @@ -239,9 +241,9 @@ def test_vary_steering(self): # Check that different steering values turn the ca sim.run(1) self.assertGreater(ego.state.rotation.y, initialAngle) - def test_bounding_box_size(self): # Check that the bounding box is calculated properly and is reasonable + def test_bounding_box_size(self): # Check that the bounding box is calculated properly and is reasonable with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) bBox = ego.bounding_box self.assertAlmostEqual(bBox.size.x, abs(bBox.max.x-bBox.min.x)) self.assertAlmostEqual(bBox.size.y, abs(bBox.max.y-bBox.min.y)) @@ -250,32 +252,32 @@ def test_bounding_box_size(self): # Check that the bounding box is calculated pr self.assertLess(bBox.size.y, 10) self.assertLess(bBox.size.z, 10) - def test_bounding_box_center(self): # Check that the bounding box center is calcualted properly + def test_bounding_box_center(self): # Check that the bounding box center is calcualted properly with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) bBox = ego.bounding_box self.assertAlmostEqual(bBox.center.x, (bBox.max.x+bBox.min.x)/2) self.assertAlmostEqual(bBox.center.y, (bBox.max.y+bBox.min.y)/2) self.assertAlmostEqual(bBox.center.z, (bBox.max.z+bBox.min.z)/2) - def test_equality(self): # Check that agent == operation works + def test_equality(self): # Check that agent == operation works with SimConnection() as sim: - ego1 = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) - ego2 = sim.add_agent("Jaguar2015XE (Autoware)", lgsvl.AgentType.EGO, spawnState(sim)) + ego1 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) + ego2 = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai, lgsvl.AgentType.EGO, spawnState(sim)) self.assertTrue(ego1 == ego1) self.assertFalse(ego1 == ego2) @unittest.skip("Cruise Control not implemented yet") def test_set_fixed_speed(self): with SimConnection(60) as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) ego.set_fixed_speed(True, 15.0) self.assertAlmostEqual(ego.state.speed, 0, delta=0.001) sim.run(5) self.assertAlmostEqual(ego.state.speed, 15, delta=1) - def create_EGO(self, sim): # Only create an EGO is none are already spawned - return sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + def create_EGO(self, sim): # Only create an EGO is none are already spawned + return sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) # def test_large_throttle(self): # with SimConnection(60) as sim: diff --git a/tests/test_NPC.py b/tests/test_NPC.py index 0d45c67..dcd693c 100644 --- a/tests/test_NPC.py +++ b/tests/test_NPC.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -7,29 +7,30 @@ import unittest import time import lgsvl -from .common import SimConnection, spawnState, cmEqual, mEqual, TestTimeout, TestException +from .common import SimConnection, spawnState, cmEqual, mEqual, TestException PROBLEM = "Object reference not set to an instance of an object" # TODO Add tests for callbacks for when NPC changes lanes, reaches stop line + class TestNPC(unittest.TestCase): -# THIS TEST RUNS FIRST + # THIS TEST RUNS FIRST def test_AAA_NPC_no_scene(self): with SimConnection(load_scene=False) as sim: with self.assertRaises(Exception) as e: state = lgsvl.AgentState() agent = sim.add_agent("Jeep", lgsvl.AgentType.NPC, state) agent.state.position - self.assertFalse(repr(e.exception).startswith(PROBLEM)) + self.assertFalse(repr(e.exception).startswith(PROBLEM)) - def test_NPC_creation(self): # Check if the different types of NPCs can be created + def test_NPC_creation(self): # Check if the different types of NPCs can be created with SimConnection(60) as sim: state = spawnState(sim) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position + 10 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() for name in ["Sedan", "SUV", "Jeep", "Hatchback", "SchoolBus", "BoxTruck"]: agent = self.create_NPC(sim, name) @@ -42,75 +43,75 @@ def test_get_agents(self): state = spawnState(sim) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position + 10 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) for name in ["Sedan", "SUV", "Jeep", "Hatchback", "SchoolBus", "BoxTruck"]: self.create_NPC(sim, name) agentCount += 1 agents = sim.get_agents() self.assertEqual(len(agents), agentCount) - agentCounter = {"Jaguar2015XE (Apollo 3.0)":0, "Sedan":0, "SUV":0, "Jeep":0, "Hatchback":0, "SchoolBus":0, "BoxTruck":0} + agentCounter = {lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5:0, "Sedan":0, "SUV":0, "Jeep":0, "Hatchback":0, "SchoolBus":0, "BoxTruck":0} for a in agents: agentCounter[a.name] += 1 - expectedAgents = ["Jaguar2015XE (Apollo 3.0)", "Sedan", "SUV", "Jeep", "Hatchback", "SchoolBus", "BoxTruck"] + expectedAgents = [lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, "Sedan", "SUV", "Jeep", "Hatchback", "SchoolBus", "BoxTruck"] for a in expectedAgents: with self.subTest(a): self.assertEqual(agentCounter[a], 1) - def test_NPC_follow_lane(self): #Check if NPC can follow lane + def test_NPC_follow_lane(self): # Check if NPC can follow lane with SimConnection() as sim: state = spawnState(sim) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) agent = self.create_NPC(sim, "Sedan") agent.follow_closest_lane(True, 5.6) sim.run(2.0) agentState = agent.state self.assertGreater(agentState.speed, 0) - # self.assertAlmostEqual(agent.state.speed, 5.6, delta=1) - self.assertLess(agent.state.position.x - sim.get_spawn()[0].position.x, 5.6) + # self.assertAlmostEqual(agent.state.speed, 5.6, delta=1) + self.assertLess(agent.state.position.x - sim.get_spawn()[0].position.x, 5.6*2) - def test_rotate_NPC(self): # Check if NPC can be rotated + def test_rotate_NPC(self): # Check if NPC can be rotated with SimConnection() as sim: state = spawnState(sim) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) agent = self.create_NPC(sim, "SUV") - self.assertAlmostEqual(agent.state.transform.rotation.y, 194.823394, places=3) + self.assertAlmostEqual(agent.state.transform.rotation.y, 104.823394, places=3) x = agent.state - x.transform.rotation.y = 100 + x.transform.rotation.y = 10 agent.state = x - self.assertAlmostEqual(agent.state.transform.rotation.y, 100, delta=0.1) + self.assertAlmostEqual(agent.state.transform.rotation.y, 10, delta=0.1) - def test_blank_agent(self): # Check that an exception is raised if a blank name is given + def test_blank_agent(self): # Check that an exception is raised if a blank name is given with SimConnection() as sim: with self.assertRaises(Exception) as e: self.create_NPC(sim, "") self.assertFalse(repr(e.exception).startswith(PROBLEM)) - def test_int_agent(self): # Check that an exception is raised if an integer name is given + def test_int_agent(self): # Check that an exception is raised if an integer name is given with SimConnection() as sim: with self.assertRaises(TypeError): self.create_NPC(sim, 1) - def test_wrong_type_NPC(self): # Check that an exception is raised if 4 is given as the agent type + def test_wrong_type_NPC(self): # Check that an exception is raised if 4 is given as the agent type with SimConnection() as sim: with self.assertRaises(TypeError): sim.add_agent("SUV", 4, spawnState(sim)) - + def test_wrong_type_value(self): with SimConnection() as sim: with self.assertRaises(ValueError): sim.add_agent("SUV", lgsvl.AgentType(9), spawnState(sim)) - def test_upsidedown_NPC(self): # Check that an upside-down NPC keeps falling + def test_upsidedown_NPC(self): # Check that an upside-down NPC keeps falling with SimConnection() as sim: state = spawnState(sim) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position + 10 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) state = spawnState(sim) state.rotation.z += 180 agent = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) @@ -119,13 +120,13 @@ def test_upsidedown_NPC(self): # Check that an upside-down NPC keeps falling final_height = agent.state.position.y self.assertLess(final_height, initial_height) - def test_flying_NPC(self): # Check if an NPC created above the map falls + def test_flying_NPC(self): # Check if an NPC created above the map falls with SimConnection() as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) up = lgsvl.utils.transform_to_up(state.transform) state.transform.position = state.position - 10 * forward + 200 * up - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) state = spawnState(sim) state.transform.position = state.position + 200 * up agent = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) @@ -134,7 +135,7 @@ def test_flying_NPC(self): # Check if an NPC created above the map falls final_height = agent.state.position.y self.assertLess(final_height, initial_height) - def test_underground_NPC(self): # Check if an NPC created below the map keeps falling + def test_underground_NPC(self): # Check if an NPC created below the map keeps falling with SimConnection() as sim: state = spawnState(sim) up = lgsvl.utils.transform_to_up(state.transform) @@ -145,7 +146,7 @@ def test_underground_NPC(self): # Check if an NPC created below the map keeps fa final_height = agent.state.position.y self.assertLess(final_height, initial_height) - def test_access_removed_NPC(self): # Check that and exception is raised when trying to access position of a removed NPC + def test_access_removed_NPC(self): # Check that and exception is raised when trying to access position of a removed NPC with SimConnection() as sim: state = spawnState(sim) agent = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) @@ -155,16 +156,16 @@ def test_access_removed_NPC(self): # Check that and exception is raised when try agent.state.position self.assertFalse(repr(e.exception).startswith(PROBLEM)) - def test_follow_waypoints(self): # Check that the NPC can follow waypoints + def test_follow_waypoints(self): # Check that the NPC can follow waypoints with SimConnection(60) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() agent = self.create_NPC(sim, "Sedan") - + # snake-drive layer_mask = 0 layer_mask |= 1 << 0 @@ -194,14 +195,14 @@ def on_waypoint(agent, index): sim.run() - def test_high_waypoint(self): # Check that a NPC will drive to under a high waypoint + def test_high_waypoint(self): # Check that a NPC will drive to under a high waypoint with SimConnection(15) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) up = lgsvl.utils.transform_to_up(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) spawns = sim.get_spawn() agent = self.create_NPC(sim, "Sedan") @@ -221,7 +222,7 @@ def on_waypoint(agent,index): self.assertLess((agent.state.position - destination).magnitude(), 1) - def test_npc_different_directions(self): # Check that specified velocities match the NPC's movement + def test_npc_different_directions(self): # Check that specified velocities match the NPC's movement with SimConnection() as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) @@ -229,7 +230,7 @@ def test_npc_different_directions(self): # Check that specified velocities match right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) state = spawnState(sim) state.velocity = -10 * right npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) @@ -251,7 +252,7 @@ def test_npc_different_directions(self): # Check that specified velocities match target = state.position + 4 * forward self.assertLess((npc.state.position - target).magnitude(), 1) - def test_stopline_callback(self): # Check that the stopline call back works properly + def test_stopline_callback(self): # Check that the stopline call back works properly with self.assertRaises(TestException) as e: with SimConnection(60) as sim: state = spawnState(sim) @@ -266,11 +267,11 @@ def on_stop_line(agent): right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 5 * right - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sim.run(60) self.assertIn("Waypoint reached", repr(e.exception)) - def test_remove_npc_with_callback(self): # Check that an NPC with callbacks is removed properly + def test_remove_npc_with_callback(self): # Check that an NPC with callbacks is removed properly with SimConnection() as sim: npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, spawnState(sim)) @@ -287,9 +288,9 @@ def on_stop_line(agent): with self.assertRaises(KeyError): sim.callbacks[npc] - def test_spawn_speed(self): # Checks that a spawned agent keeps the correct speed when spawned + def test_spawn_speed(self): # Checks that a spawned agent keeps the correct speed when spawned with SimConnection() as sim: - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim, 1)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim, 1)) npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, spawnState(sim)) self.assertEqual(npc.state.speed,0) @@ -299,20 +300,21 @@ def test_spawn_speed(self): # Checks that a spawned agent keeps the correct spee def test_lane_change_right(self): with SimConnection(40) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49)) + state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) - state.transform = sim.map_point_on_lane(lgsvl.Vector(42.73, -1.57, -0.63)) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform = sim.map_point_on_lane(lgsvl.Vector(0.63, -1.57, 42.73)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 31 * forward npc.follow_closest_lane(True, 10) agents = [] + def on_lane_change(agent): agents.append(agent) - + npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(False) @@ -324,20 +326,21 @@ def on_lane_change(agent): def test_lane_change_right_missing_lane(self): with SimConnection(40) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(42.73, -1.57, -0.63)) + state.transform = sim.map_point_on_lane(lgsvl.Vector(0.63, -1.57, 42.73)) npc = sim.add_agent("Hatchback", lgsvl.AgentType.NPC, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 42.75 * forward - state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49)) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) npc.follow_closest_lane(True, 10) agents = [] + def on_lane_change(agent): agents.append(agent) - + npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(False) @@ -349,20 +352,21 @@ def on_lane_change(agent): def test_lane_change_left(self): with SimConnection(40) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(40.85, -1.57, -4.49)) + state.transform = sim.map_point_on_lane(lgsvl.Vector(4.49, -1.57, 40.85)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) - state.transform = sim.map_point_on_lane(lgsvl.Vector(42.02, -1.79, -9.82)) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform = sim.map_point_on_lane(lgsvl.Vector(9.82, -1.79, 42.02)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 31 * forward npc.follow_closest_lane(True, 10) agents = [] + def on_lane_change(agent): agents.append(agent) - + npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(True) @@ -374,20 +378,21 @@ def on_lane_change(agent): def test_lane_change_left_opposing_traffic(self): with SimConnection(40) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962)) + state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292)) npc = sim.add_agent("SUV", lgsvl.AgentType.NPC, state) forward = lgsvl.utils.transform_to_forward(state.transform) target = state.position + 42.75 * forward state.transform.position = state.position - 10 * forward - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) npc.follow_closest_lane(True, 10) agents = [] + def on_lane_change(agent): agents.append(agent) - + npc.on_lane_change(on_lane_change) sim.run(2) npc.change_lane(True) @@ -400,18 +405,19 @@ def on_lane_change(agent): def test_multiple_lane_changes(self): with SimConnection(120) as sim: state = lgsvl.AgentState() - state.transform.position = lgsvl.Vector(239,10,180) - state.transform.rotation = lgsvl.Vector(0,180,0) - sim.add_agent("XE_Rigged-apollo", lgsvl.AgentType.EGO, state) + state.transform.position = lgsvl.Vector(-180,10,239) + state.transform.rotation = lgsvl.Vector(0,90,0) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai, lgsvl.AgentType.EGO, state) state = lgsvl.AgentState() - state.transform.position = lgsvl.Vector(234.5, 10, 175) - state.transform.rotation = lgsvl.Vector(0.016, 180, 0) + state.transform.position = lgsvl.Vector(-175, 10, 234.5) + state.transform.rotation = lgsvl.Vector(0, 90, 0.016) npc = sim.add_agent("Sedan", lgsvl.AgentType.NPC, state) npc.follow_closest_lane(True, 10) agents = [] + def on_lane_change(agent): agents.append(agent) @@ -441,7 +447,7 @@ def test_set_lights_exceptions(self): control = lgsvl.NPCControl() control.headlights = 2 npc.apply_control(control) - + with self.assertRaises(ValueError): control.headlights = 15 npc.apply_control(control) @@ -455,10 +461,10 @@ def test_npc_control_exceptions(self): npc.apply_control(control) def test_e_stop(self): - with SimConnection() as sim: + with SimConnection(60) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962)) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) state.transform.position = state.position + 10 * forward npc = sim.add_agent("Jeep", lgsvl.AgentType.NPC, state) @@ -478,8 +484,8 @@ def test_e_stop(self): def test_waypoint_speed(self): with SimConnection(60) as sim: state = lgsvl.AgentState() - state.transform = sim.map_point_on_lane(lgsvl.Vector(-40.292, -3.363, -78.962)) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + state.transform = sim.map_point_on_lane(lgsvl.Vector(78.962, -3.363, -40.292)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) forward = lgsvl.utils.transform_to_forward(state.transform) up = lgsvl.utils.transform_to_up(state.transform) state.transform.position = state.position + 10 * forward @@ -535,5 +541,5 @@ def on_waypoint(agent, index): # sim.run(2) # self.assertAlmostEqual(npc.state.speed, 8, delta=1) - def create_NPC(self, sim, name): # Create the specified NPC + def create_NPC(self, sim, name): # Create the specified NPC return sim.add_agent(name, lgsvl.AgentType.NPC, spawnState(sim)) diff --git a/tests/test_collisions.py b/tests/test_collisions.py index 8f79a1e..c24ae6c 100644 --- a/tests/test_collisions.py +++ b/tests/test_collisions.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -11,16 +11,17 @@ # TODO add tests for collisions between NPCs, EGO & obstacles + class TestCollisions(unittest.TestCase): - def test_ego_collision(self): # Check that a collision between Ego and NPC is reported + def test_ego_collision(self): # Check that a collision between Ego and NPC is reported with SimConnection() as sim: - mover, bus = self.setup_collision(sim, "Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) + mover, bus = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) collisions = [] def on_collision(agent1, agent2, contact): collisions.append([agent1, agent2, contact]) sim.stop() - + mover.on_collision(on_collision) bus.on_collision(on_collision) @@ -29,18 +30,18 @@ def on_collision(agent1, agent2, contact): self.assertGreater(len(collisions), 0) self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ego Collision") - self.assertTrue(collisions[0][0].name == "Jaguar2015XE (Apollo 3.0)" or collisions[0][1].name == "Jaguar2015XE (Apollo 3.0)") + self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5 or collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) self.assertTrue(True) - def test_sim_stop(self): # Check that sim.stop works properly + def test_sim_stop(self): # Check that sim.stop works properly with SimConnection() as sim: - mover, bus = self.setup_collision(sim, "Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) + mover, bus = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "SchoolBus", lgsvl.AgentType.NPC) collisions = [] def on_collision(agent1, agent2, contact): collisions.append([agent1, agent2, contact]) sim.stop() - + mover.on_collision(on_collision) bus.on_collision(on_collision) @@ -48,9 +49,9 @@ def on_collision(agent1, agent2, contact): self.assertLess(sim.current_time, 15.0) - def test_ped_collision(self): # Check if a collision between EGO and pedestrian is reported + def test_ped_collision(self): # Check if a collision between EGO and pedestrian is reported with SimConnection() as sim: - ego, ped = self.setup_collision(sim, "Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, "Howard", lgsvl.AgentType.PEDESTRIAN) + ego, ped = self.setup_collision(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, "Howard", lgsvl.AgentType.PEDESTRIAN) self.assertTrue(isinstance(ego, lgsvl.EgoVehicle)) self.assertTrue(isinstance(ped, lgsvl.Pedestrian)) collisions = [] @@ -64,10 +65,10 @@ def on_collision(agent1, agent2, contact): sim.run(15) self.assertGreater(len(collisions), 0) self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ped Collision") - self.assertTrue(collisions[0][0].name == "Jaguar2015XE (Apollo 3.0)" or collisions[0][1].name == "Jaguar2015XE (Apollo 3.0)") + self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5 or collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) @unittest.skip("NPCs ignore collisions with Pedestrians, activate this when NPCs use real physics") - def test_ped_npc_collisions(self): # Check that collision between NPC and Pedestrian is reported + def test_ped_npc_collisions(self): # Check that collision between NPC and Pedestrian is reported with SimConnection() as sim: state = spawnState(sim) state.position.y += 10 @@ -82,24 +83,24 @@ def on_collision(agent1, agent2, contact): ped.on_collision(on_collision) bus.on_collision(on_collision) sim.run(15) - + self.assertGreater(len(collisions), 0) self.assertInBetween(collisions[0][2], collisions[0][0].state.position, collisions[0][1].state.position, "Ped/NPC Collision") self.assertTrue(collisions[0][0].name == "Bob" or collisions[0][1].name == "Bob") @unittest.skip("NPCs ignore collisions with other NPCs, activate this when NPCs use real physics") - def test_npc_collision(self): # Check that collision between NPC and NPC is reported + def test_npc_collision(self): # Check that collision between NPC and NPC is reported with SimConnection() as sim: state = spawnState(sim) state.position.x += 10 - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) jeep, bus = self.setup_collision(sim, "Jeep", lgsvl.AgentType.NPC, "SchoolBus", lgsvl.AgentType.NPC) collisions = [] def on_collision(agent1, agent2, contact): collisions.append([agent1, agent2, contact]) sim.stop() - + jeep.on_collision(on_collision) bus.on_collision(on_collision) @@ -110,7 +111,7 @@ def on_collision(agent1, agent2, contact): self.assertTrue(collisions[0][0].name == "Jeep" or collisions[0][1].name == "Jeep") self.assertTrue(collisions[0][0].name == "SchoolBus" or collisions[0][1].name == "SchoolBus") - def test_wall_collision(self): # Check that an EGO collision with a wall is reported properly + def test_wall_collision(self): # Check that an EGO collision with a wall is reported properly with SimConnection() as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) @@ -118,26 +119,26 @@ def test_wall_collision(self): # Check that an EGO collision with a wall is repo right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position + 30 * right - 1 * up + 140 * forward state.velocity = 50 * forward - ego = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) collisions = [] def on_collision(agent1, agent2, contact): collisions.append([agent1, agent2, contact]) sim.stop() - + ego.on_collision(on_collision) sim.run(15) self.assertGreater(len(collisions), 0) if collisions[0][0] is None: - self.assertTrue(collisions[0][1].name == "Jaguar2015XE (Apollo 3.0)") + self.assertTrue(collisions[0][1].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) elif collisions[0][1] is None: - self.assertTrue(collisions[0][0].name == "Jaguar2015XE (Apollo 3.0)") + self.assertTrue(collisions[0][0].name == lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) else: self.fail("Collision not with object") - def setup_collision(self, sim, mover_name, agent_type, still_name, still_type): + def setup_collision(self, sim, mover_name, agent_type, still_name, still_type): # Creates 2 agents, the mover is created with a forward velocity # still is rotated 90 degree in and in front of the mover state = spawnState(sim) @@ -154,12 +155,12 @@ def setup_collision(self, sim, mover_name, agent_type, still_name, still_type): return mover, still - def assertInBetween(self, position, a, b, msg): # Tests that at least one component of the input position vector is between the a and b vectors + def assertInBetween(self, position, a, b, msg): # Tests that at least one component of the input position vector is between the a and b vectors xmid = (a.x+b.x)/2 xdiff = abs(a.x-xmid) xmin = xmid-xdiff xmax = xmid+xdiff - + ymid = (a.y+b.y)/2 ydiff = abs(a.y-ymid) ymin = ymid-ydiff diff --git a/tests/test_manual_features.py b/tests/test_manual_features.py index a01c038..c9be960 100644 --- a/tests/test_manual_features.py +++ b/tests/test_manual_features.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -7,16 +7,16 @@ # DO NOT INCLUDE IN __init__.py import unittest -import math import lgsvl from .common import SimConnection, spawnState, TestTimeout class TestManual(unittest.TestCase): + @unittest.skip("Windshield wipers no longer supported") def test_wipers(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.windshield_wipers = 1 ego.apply_control(control, True) @@ -36,11 +36,11 @@ def test_wipers(self): input("Press Enter if wipers are on high") except TestTimeout: self.fail("Wipers were not on") - + def test_headlights(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.headlights = 1 ego.apply_control(control, True) @@ -58,7 +58,7 @@ def test_headlights(self): def test_blinkers(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.turn_signal_left = True ego.apply_control(control, True) @@ -73,10 +73,11 @@ def test_blinkers(self): except TestTimeout: self.fail("Turn signals were not on") + @unittest.skip("Windshield wipers no longer supported") def test_wiper_large_value(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.windshield_wipers = 4 ego.apply_control(control, True) @@ -85,10 +86,11 @@ def test_wiper_large_value(self): except TestTimeout: self.fail("Wipers were on") + @unittest.skip("Windshield wipers no longer supported") def test_wiper_str(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.windshield_wipers = "on" ego.apply_control(control, True) @@ -100,7 +102,7 @@ def test_wiper_str(self): def test_headlights_large_value(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.headlights = 123 ego.apply_control(control, True) @@ -112,7 +114,7 @@ def test_headlights_large_value(self): def test_headlights_str(self): try: with SimConnection() as sim: - ego = sim.add_agent("Jaguar2015XE (Apollo 3.5)", lgsvl.AgentType.EGO, spawnState(sim)) + ego = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) control = lgsvl.VehicleControl() control.headlights = "123" ego.apply_control(control, True) diff --git a/tests/test_peds.py b/tests/test_peds.py index 06fa731..d49fb5d 100644 --- a/tests/test_peds.py +++ b/tests/test_peds.py @@ -1,35 +1,35 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # import unittest import math -import time import lgsvl from .common import SimConnection, cmEqual, mEqual, spawnState + class TestPeds(unittest.TestCase): - def test_ped_creation(self): # Check if the different types of Peds can be created + def test_ped_creation(self): # Check if the different types of Peds can be created with SimConnection() as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) state.transform.position = state.position - 4 * forward - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) for name in ["Bob", "EntrepreneurFemale", "Howard", "Johny", \ - "Pamela", "Presley", "Red", "Robin", "Stephen", "Zoe"]: + "Pamela", "Presley", "Robin", "Stephen", "Zoe"]: agent = self.create_ped(sim, name, spawnState(sim)) cmEqual(self, agent.state.position, sim.get_spawn()[0].position, name) self.assertEqual(agent.name, name) - - def test_ped_random_walk(self): # Check if pedestrians can walk randomly - with SimConnection() as sim: + + def test_ped_random_walk(self): # Check if pedestrians can walk randomly + with SimConnection(40) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) state.transform.position = state.position - 4 * forward - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) state = spawnState(sim) spawnPoint = state.transform.position @@ -47,13 +47,13 @@ def test_ped_random_walk(self): # Check if pedestrians can walk randomly cmEqual(self, randPoint, bob.state.transform.position, "Ped random walk") - def test_ped_circle_waypoints(self): # Check if pedestrians can follow waypoints + def test_ped_circle_waypoints(self): # Check if pedestrians can follow waypoints with SimConnection(60) as sim: state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) state.transform.position = state.position - 4 * forward - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) state = spawnState(sim) state.transform.position = state.position + 10 * forward radius = 5 @@ -71,6 +71,7 @@ def test_ped_circle_waypoints(self): # Check if pedestrians can follow waypoints state.transform.position = waypoints[0] zoe = self.create_ped(sim, "Zoe", state) + def on_waypoint(agent,index): msg = "Waypoint " + str(index) mEqual(self, zoe.state.position, waypoints[index], msg) @@ -83,7 +84,7 @@ def on_waypoint(agent,index): def test_waypoint_idle_time(self): with SimConnection(60) as sim: - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) state = spawnState(sim) forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) @@ -102,10 +103,10 @@ def on_waypoint(agent, index): zoe.on_waypoint_reached(on_waypoint) zoe.follow(waypoints) - t0 = time.time() + t0 = sim.current_time sim.run() # reach the first waypoint sim.run() # reach the second waypoint - t1 = time.time() + t1 = sim.current_time noIdleTime = t1-t0 zoe.state = state @@ -115,13 +116,13 @@ def on_waypoint(agent, index): hit = sim.raycast(state.position - 5 * right, lgsvl.Vector(0, -1, 0), layer_mask) waypoints.append(lgsvl.WalkWaypoint(hit.point, 0)) zoe.follow(waypoints) - t2 = time.time() + t2 = sim.current_time sim.run() # reach the first waypoint sim.run() # reach the second waypoint - t3 = time.time() + t3 = sim.current_time idleTime = t3-t2 self.assertAlmostEqual(idleTime-noIdleTime, 2.0, delta=0.5) - def create_ped(self, sim, name, state): # create the specified Pedestrian + def create_ped(self, sim, name, state): # create the specified Pedestrian return sim.add_agent(name, lgsvl.AgentType.PEDESTRIAN, state) diff --git a/tests/test_sensors.py b/tests/test_sensors.py index 545ec71..e890b7d 100644 --- a/tests/test_sensors.py +++ b/tests/test_sensors.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -11,10 +11,11 @@ # TODO add tests for bridge to check if enabled sensor actually sends data + class TestSensors(unittest.TestCase): - def test_apollo_5_sensors(self): # Check that Apollo 3.5 sensors are created and are positioned reasonably + def test_apollo_5_sensors(self): # Check that Apollo 3.5 sensors are created and are positioned reasonably with SimConnection(60) as sim: - agent = self.create_EGO(sim, "Lincoln2017MKZ (Apollo 5.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) expectedSensors = ['Lidar', 'GPS', 'Telephoto Camera', \ 'Main Camera', 'IMU', 'CAN Bus', 'Radar'] sensors = agent.get_sensors() @@ -30,9 +31,9 @@ def test_apollo_5_sensors(self): # Check that Apollo 3.5 sensors are created and self.valid_sensor(s, msg) @unittest.skip("No SF vehicle") - def test_santafe_sensors(self): # Check that Apollo Santa Fe sensors are created and are positioned reasonably + def test_santafe_sensors(self): # Check that Apollo Santa Fe sensors are created and are positioned reasonably with SimConnection() as sim: - agent = self.create_EGO(sim, "Lincoln2017MKZ (Apollo 5.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] sensors = agent.get_sensors() @@ -48,9 +49,9 @@ def test_santafe_sensors(self): # Check that Apollo Santa Fe sensors are created self.valid_sensor(s, msg) @unittest.skip("No LGSVL Vehicle") - def test_lgsvl_sensors(self): # Check that LGSVL sensors are created and are positioned reasonably + def test_lgsvl_sensors(self): # Check that LGSVL sensors are created and are positioned reasonably with SimConnection() as sim: - agent = self.create_EGO(sim, "Lincoln2017MKZ (Apollo 5.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_lincoln2017mkz_apollo5) expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] sensors = agent.get_sensors() @@ -66,14 +67,14 @@ def test_lgsvl_sensors(self): # Check that LGSVL sensors are created and are pos self.valid_sensor(s, msg) @unittest.skip("No EP Vehicle") - def test_ep_sensors(self): # Check that Apollo EP sensors are created and are positioned reasonably + def test_ep_sensors(self): # Check that Apollo EP sensors are created and are positioned reasonably with SimConnection() as sim: agent = self.create_EGO(sim, "EP_Rigged-apollo") expectedSensors = ['velodyne', 'GPS', 'Telephoto Camera', 'Main Camera', \ 'IMU', 'RADAR', 'CANBUS', 'Segmentation Camera', 'Left Camera', 'Right Camera'] sensors = agent.get_sensors() sensorNames = [s.name for s in sensors] - + for sensor in expectedSensors: with self.subTest(sensor): self.assertIn(sensor, sensorNames) @@ -83,9 +84,9 @@ def test_ep_sensors(self): # Check that Apollo EP sensors are created and are po with self.subTest(msg): self.valid_sensor(s, msg) - def test_apollo_sensors(self): # Check that all the Apollo sensors are there + def test_apollo_sensors(self): # Check that all the Apollo sensors are there with SimConnection() as sim: - agent = self.create_EGO(sim, "Jaguar2015XE (Apollo 3.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) expectedSensors = ["Lidar", "GPS", "Telephoto Camera", "Main Camera", "IMU", \ "CAN Bus", "Radar"] @@ -101,9 +102,9 @@ def test_apollo_sensors(self): # Check that all the Apollo sensors are there with self.subTest(msg): self.valid_sensor(s, msg) - def test_autoware_sensors(self): # Check that all Autoware sensors are there + def test_autoware_sensors(self): # Check that all Autoware sensors are there with SimConnection() as sim: - agent = self.create_EGO(sim, "Jaguar2015XE (Autoware)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguae2015xe_autowareai) expectedSensors = ["Lidar", "GPS", "Main Camera", "IMU"] sensors = agent.get_sensors() @@ -118,18 +119,18 @@ def test_autoware_sensors(self): # Check that all Autoware sensors are there with self.subTest(msg): self.valid_sensor(s, msg ) - def test_save_sensor(self): # Check that sensor results can be saved + def test_save_sensor(self): # Check that sensor results can be saved with SimConnection(120) as sim: path = "main-camera.png" - islocal = os.environ.get("SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" + islocal = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" if islocal: path = os.getcwd() + path if os.path.isfile(path): os.remove(path) - agent = self.create_EGO(sim, "Jaguar2015XE (Apollo 3.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) sensors = agent.get_sensors() savedSuccess = False @@ -146,17 +147,17 @@ def test_save_sensor(self): # Check that sensor results can be saved self.assertGreater(os.path.getsize(path), 0) os.remove(path) - def test_save_lidar(self): # Check that LIDAR sensor results can be saved + def test_save_lidar(self): # Check that LIDAR sensor results can be saved with SimConnection(240) as sim: path = "lidar.pcd" - islocal = os.environ.get("SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" + islocal = os.environ.get("LGSVL__SIMULATOR_HOST", "127.0.0.1") == "127.0.0.1" if islocal: path = os.getcwd() + path if os.path.isfile(path): os.remove(path) - agent = self.create_EGO(sim, "Jaguar2015XE (Apollo 3.0)") + agent = self.create_EGO(sim, lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5) sensors = agent.get_sensors() savedSuccess = False @@ -172,14 +173,13 @@ def test_save_lidar(self): # Check that LIDAR sensor results can be saved self.assertGreater(os.path.getsize(path), 0) os.remove(path) - - def test_GPS(self): # Check that the GPS sensor works + def test_GPS(self): # Check that the GPS sensor works with SimConnection() as sim: state = lgsvl.AgentState() state.transform = sim.get_spawn()[0] right = lgsvl.utils.transform_to_right(state.transform) state.velocity = -50 * right - agent = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) sensors = agent.get_sensors() initialGPSData = None gps = None @@ -203,13 +203,13 @@ def test_GPS(self): # Check that the GPS sensor works self.assertNotAlmostEqual(gps.data.altitude, 0) self.assertNotAlmostEqual(gps.data.orientation, 0) - def test_sensor_disabling(self): # Check if sensors can be enabled + def test_sensor_disabling(self): # Check if sensors can be enabled with SimConnection() as sim: - agent = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) for s in agent.get_sensors(): if s.name == "Lidar": s.enabled = False - + for s in agent.get_sensors(): with self.subTest(s.name): if (s.name == "Lidar"): @@ -217,9 +217,9 @@ def test_sensor_disabling(self): # Check if sensors can be enabled else: self.assertTrue(s.enabled) - def test_sensor_equality(self): # Check that sensor == operator works + def test_sensor_equality(self): # Check that sensor == operator works with SimConnection() as sim: - agent = sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + agent = sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) prevSensor = None for s in agent.get_sensors(): self.assertTrue(s == s) @@ -227,14 +227,14 @@ def test_sensor_equality(self): # Check that sensor == operator works self.assertFalse(s == prevSensor) prevSensor = s - def create_EGO(self, sim, name): # Creates the speicified EGO and removes any others + def create_EGO(self, sim, name): # Creates the speicified EGO and removes any others return sim.add_agent(name, lgsvl.AgentType.EGO, spawnState(sim)) - def valid_sensor(self, sensor, msg): # Checks that the sensor is close to the EGO and not overly rotated + def valid_sensor(self, sensor, msg): # Checks that the sensor is close to the EGO and not overly rotated self.assertBetween(sensor.transform.rotation, 0, 360, msg) - self.assertBetween(sensor.transform.position, -5, 5, msg) + self.assertBetween(sensor.transform.position, -10, 10, msg) - def assertBetween(self, vector, min, max, msg): # Tests that the given vectors components are within the min and max + def assertBetween(self, vector, min, max, msg): # Tests that the given vectors components are within the min and max self.assertGreaterEqual(vector.x, min, msg) self.assertLessEqual(vector.x, max, msg) diff --git a/tests/test_simulator.py b/tests/test_simulator.py index da65a9c..ca46998 100644 --- a/tests/test_simulator.py +++ b/tests/test_simulator.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2021 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -7,46 +7,46 @@ import unittest import lgsvl -import time -from .common import SimConnection, spawnState, TestTimeout +from .common import SimConnection, spawnState PROBLEM = "Object reference not set to an instance of an object" + class TestSimulator(unittest.TestCase): - def test_scene(self): # Check if the right scene was loaded + def test_scene(self): # Check if the right scene was loaded with SimConnection() as sim: self.assertEqual(sim.current_scene, "BorregasAve") - - def test_unload_scene(self): # Check if a different scene gets loaded + + def test_unload_scene(self): # Check if a different scene gets loaded with SimConnection() as sim: self.assertEqual(sim.current_scene, "BorregasAve") sim.load("CubeTown") self.assertEqual(sim.current_scene, "CubeTown") - def test_spawns(self): # Check if there is at least 1 spawn point for Ego Vehicles + def test_spawns(self): # Check if there is at least 1 spawn point for Ego Vehicles with SimConnection() as sim: spawns = sim.get_spawn() self.assertGreater(len(spawns), 0) - def test_run_time(self): # Check if the simulator runs 2 seconds + def test_run_time(self): # Check if the simulator runs 2 seconds with SimConnection() as sim: - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, spawnState(sim)) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, spawnState(sim)) time = 2.0 initial_time = sim.current_time sim.run(time) self.assertAlmostEqual(sim.current_time - initial_time, time, delta=0.1) - def test_non_realtime(self): # Check if non-realtime simulation runs + def test_non_realtime(self): # Check if non-realtime simulation runs with SimConnection() as sim: initial_time = sim.current_time sim.run(time_limit=3, time_scale=2) self.assertAlmostEqual(sim.current_time-initial_time, 3, delta=0.1) - def test_reset(self): # Check if sim.reset resets the time and frame + def test_reset(self): # Check if sim.reset resets the time and frame with SimConnection() as sim: sim.run(1.0) @@ -55,7 +55,7 @@ def test_reset(self): # Check if sim.reset resets the time and frame self.assertAlmostEqual(sim.current_time, 0) self.assertEqual(sim.current_frame, 0) - def test_raycast(self): # Check if raycasting works + def test_raycast(self): # Check if raycasting works with SimConnection() as sim: spawns = sim.get_spawn() state = lgsvl.AgentState() @@ -63,29 +63,29 @@ def test_raycast(self): # Check if raycasting works forward = lgsvl.utils.transform_to_forward(state.transform) right = lgsvl.utils.transform_to_right(state.transform) up = lgsvl.utils.transform_to_up(state.transform) - sim.add_agent("Jaguar2015XE (Apollo 3.0)", lgsvl.AgentType.EGO, state) + sim.add_agent(lgsvl.wise.DefaultAssets.ego_jaguar2015xe_apollo5, lgsvl.AgentType.EGO, state) p = spawns[0].position p.y += 1 layer_mask = 0 - for bit in [0, 4, 13, 14, 18]: # do not put 8 here, to not hit EGO vehicle itself + for bit in [0, 4, 13, 14, 18]: # do not put 8 here, to not hit EGO vehicle itself layer_mask |= 1 << bit # Right hit = sim.raycast(p, right, layer_mask) self.assertTrue(hit) - self.assertAlmostEqual(hit.distance, 15.0892992019653) + self.assertAlmostEqual(hit.distance, 15.089282989502, places=3) - #Left + # Left hit = sim.raycast(p, -right, layer_mask) self.assertTrue(hit) - self.assertAlmostEqual(hit.distance, 19.72922706604) + self.assertAlmostEqual(hit.distance, 19.7292556762695, places=3) - #Back + # Back hit = sim.raycast(p, -forward, layer_mask) self.assertFalse(hit) - #Front + # Front hit = sim.raycast(p, forward, layer_mask) self.assertFalse(hit) @@ -96,54 +96,54 @@ def test_raycast(self): # Check if raycasting works # Down hit = sim.raycast(p, -up, layer_mask) self.assertTrue(hit) - self.assertAlmostEqual(hit.distance, 1.08376359939575) + self.assertAlmostEqual(hit.distance, 1.0837641954422, places=3) - def test_weather(self): # Check that the weather state can be read properly and changed + def test_weather(self): # Check that the weather state can be read properly and changed with SimConnection() as sim: rain = sim.weather.rain fog = sim.weather.fog wetness = sim.weather.wetness - + self.assertAlmostEqual(rain, 0) self.assertAlmostEqual(fog, 0) self.assertAlmostEqual(wetness, 0) - sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8) + sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8, cloudiness=0, damage=0) rain = sim.weather.rain fog = sim.weather.fog wetness = sim.weather.wetness - + self.assertAlmostEqual(rain, 0.5) self.assertAlmostEqual(fog, 0.3) self.assertAlmostEqual(wetness, 0.8) - def test_invalid_weather(self): # Check that the API/Unity properly handles unexpected inputs + def test_invalid_weather(self): # Check that the API/Unity properly handles unexpected inputs with SimConnection() as sim: rain = sim.weather.rain fog = sim.weather.fog wetness = sim.weather.wetness - + self.assertAlmostEqual(rain, 0) self.assertAlmostEqual(fog, 0) self.assertAlmostEqual(wetness, 0) - sim.weather = lgsvl.WeatherState(rain=1.4, fog=-3, wetness="a") + sim.weather = lgsvl.WeatherState(rain=1.4, fog=-3, wetness="a", cloudiness=0, damage=0) rain = sim.weather.rain fog = sim.weather.fog wetness = sim.weather.wetness - + self.assertAlmostEqual(rain, 1) self.assertAlmostEqual(fog, 0) self.assertAlmostEqual(wetness, 0) - sim.weather = lgsvl.WeatherState(rain=True, fog=0, wetness=0) + sim.weather = lgsvl.WeatherState(rain=True, fog=0, wetness=0, cloudiness=0, damage=0) rain = sim.weather.rain self.assertAlmostEqual(rain, 0) - def test_time_of_day(self): # Check that the time of day is reported properly and can be set + def test_time_of_day(self): # Check that the time of day is reported properly and can be set with SimConnection() as sim: # self.assertAlmostEqual(sim.time_of_day, 9, delta=0.5) sim.set_time_of_day(18.00) @@ -154,14 +154,14 @@ def test_time_of_day(self): # Check that the time of day is reported properly an sim.run(3) self.assertGreater(sim.time_of_day, 13.5) - def test_wrong_time_of_day(self): # Check that the time of day is not broken with inappropriate inputs + def test_wrong_time_of_day(self): # Check that the time of day is not broken with inappropriate inputs with SimConnection() as sim: sim.set_time_of_day(40) self.assertAlmostEqual(sim.time_of_day, 0) with self.assertRaises(TypeError): sim.set_time_of_day("asdf") - - def test_reset_weather(self): # Check that reset sets the weather variables back to 0 + + def test_reset_weather(self): # Check that reset sets the weather variables back to 0 with SimConnection() as sim: rain = sim.weather.rain fog = sim.weather.fog @@ -170,7 +170,7 @@ def test_reset_weather(self): # Check that reset sets the weather variables back self.assertAlmostEqual(fog, 0) self.assertAlmostEqual(wetness, 0) - sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8) + sim.weather = lgsvl.WeatherState(rain=0.5, fog=0.3, wetness=0.8, cloudiness=0, damage=0) rain = sim.weather.rain fog = sim.weather.fog wetness = sim.weather.wetness @@ -186,29 +186,29 @@ def test_reset_weather(self): # Check that reset sets the weather variables back self.assertAlmostEqual(fog, 0) self.assertAlmostEqual(wetness, 0) - def test_reset_time(self): # Check that reset sets time back to the default + def test_reset_time(self): # Check that reset sets time back to the default with SimConnection() as sim: default_time = sim.time_of_day sim.set_time_of_day((default_time+5)%24) sim.reset() self.assertAlmostEqual(default_time, sim.time_of_day) -# THIS TEST RUNS LAST - def test_ztypo_map(self): # Check if an exception is raised with a misspelled map name is loaded + # THIS TEST RUNS LAST + def test_ztypo_map(self): # Check if an exception is raised with a misspelled map name is loaded #with self.assertRaises(TestTimeout): with SimConnection() as sim: with self.assertRaises(Exception) as e: sim.load("SF") self.assertFalse(repr(e.exception).startswith(PROBLEM)) - def test_negative_time(self): # Check that a negative time can be handled properly + def test_negative_time(self): # Check that a negative time can be handled properly with SimConnection() as sim: initial_time = sim.current_time sim.run(-5) post_time = sim.current_time self.assertAlmostEqual(initial_time, post_time) - def test_get_gps(self): # Checks that GPS reports the correct values + def test_get_gps(self): # Checks that GPS reports the correct values with SimConnection() as sim: spawn = sim.get_spawn()[0] gps = sim.map_to_gps(spawn) @@ -217,38 +217,38 @@ def test_get_gps(self): # Checks that GPS reports the correct values self.assertAlmostEqual(gps.northing, 4141627.34000015) self.assertAlmostEqual(gps.easting, 587060.970000267) self.assertAlmostEqual(gps.altitude, -1.03600001335144) - self.assertAlmostEqual(gps.orientation, -194.823394775391) + self.assertAlmostEqual(gps.orientation, -104.823394775391) - def test_from_northing(self): # Check that position vectors are correctly generated given northing and easting + def test_from_northing(self): # Check that position vectors are correctly generated given northing and easting with SimConnection() as sim: spawn = sim.get_spawn()[0] location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267) self.assertAlmostEqual(spawn.position.x, location.position.x, places=1) self.assertAlmostEqual(spawn.position.z, location.position.z, places=1) - def test_from_latlong(self): # Check that position vectors are correctly generated given latitude and longitude + def test_from_latlong(self): # Check that position vectors are correctly generated given latitude and longitude with SimConnection() as sim: spawn = sim.get_spawn()[0] location = sim.map_from_gps(latitude=37.4173601699318, longitude=-122.016132757826) self.assertAlmostEqual(spawn.position.x, location.position.x, places=1) self.assertAlmostEqual(spawn.position.z, location.position.z, places=1) - def test_from_alt_orient(self): # Check that position vectors are correctly generated with altitude and orientation + def test_from_alt_orient(self): # Check that position vectors are correctly generated with altitude and orientation with SimConnection() as sim: spawn = sim.get_spawn()[0] - location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267, altitude=-1.03600001335144, orientation=-194.823394775391) + location = sim.map_from_gps(northing=4141627.34000015, easting=587060.970000267, altitude=-1.03600001335144, orientation=-104.823371887207) self.assertAlmostEqual(spawn.position.y, location.position.y, places=1) self.assertAlmostEqual(spawn.rotation.y, location.rotation.y, places=1) - def test_false_latlong(self): # Check that exceptions are thrown when inputting invalid lat long values + def test_false_latlong(self): # Check that exceptions are thrown when inputting invalid lat long values with SimConnection() as sim: with self.assertRaises(ValueError): sim.map_from_gps(latitude=91, longitude=0) - + with self.assertRaises(ValueError): sim.map_from_gps(latitude=0, longitude=200) - def test_false_easting(self): # Check that exceptions are thrown when inputting invalid northing easting values + def test_false_easting(self): # Check that exceptions are thrown when inputting invalid northing easting values with SimConnection() as sim: with self.assertRaises(ValueError): sim.map_from_gps(easting=1000000000, northing=500000) @@ -256,13 +256,13 @@ def test_false_easting(self): # Check that exceptions are thrown when inputting with self.assertRaises(ValueError): sim.map_from_gps(northing=-50, easting=500000) - def test_version_info(self): # Check that the sim reports a numerical version number + def test_version_info(self): # Check that the sim reports a numerical version number with SimConnection() as sim: version = sim.version self.assertTrue(isinstance(version, str)) self.assertTrue(isinstance(float(version[:4]), float)) - def test_lat_northing(self): # Checks that exceptions are thrown if an invalid pair of gps values are given + def test_lat_northing(self): # Checks that exceptions are thrown if an invalid pair of gps values are given with SimConnection() as sim: with self.assertRaises(Exception) as e: sim.map_from_gps(northing=4812775, latitude=37.7) @@ -272,33 +272,33 @@ def test_lat_northing(self): # Checks that exceptions are thrown if an invalid p # with SimConnection() as sim: # with self.assertRaises(Exception) as e: # sim.map_from_gps(northing=1, easting=2, latitude=3, longitude=4) - - def test_lat_str(self): # Checks that exceptions are thrown if a string is given for latitude + + def test_lat_str(self): # Checks that exceptions are thrown if a string is given for latitude with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(latitude="asdf", longitude=2) - def test_long_str(self): # Checks that exceptions are thrown if a string is given for longitude + def test_long_str(self): # Checks that exceptions are thrown if a string is given for longitude with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(latitude=1, longitude="asdf") - def test_northing_str(self): # Checks that exceptions are thrown if a string is given for northing + def test_northing_str(self): # Checks that exceptions are thrown if a string is given for northing with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(northing="asdf", easting=2) - def test_easting_str(self): # Checks that exceptions are thrown if a string is given for easting + def test_easting_str(self): # Checks that exceptions are thrown if a string is given for easting with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(northing=1, easting="asdF") - def test_altitude_str(self): # Checks that exceptions are thrown if a string is given for altitude + def test_altitude_str(self): # Checks that exceptions are thrown if a string is given for altitude with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(latitude=1, longitude=2, altitude="asd") - def test_orientation_str(self): # Checks that exceptions are thrown if a string is given for orientation + def test_orientation_str(self): # Checks that exceptions are thrown if a string is given for orientation with SimConnection() as sim: with self.assertRaises(TypeError): sim.map_from_gps(latitude=1, longitude=2, orientation="asdf") diff --git a/tests/test_utils.py b/tests/test_utils.py index 899d20b..91d8f66 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -1,5 +1,5 @@ # -# Copyright (c) 2019 LG Electronics, Inc. +# Copyright (c) 2019-2020 LG Electronics, Inc. # # This software contains code licensed as described in LICENSE. # @@ -9,46 +9,47 @@ import lgsvl import lgsvl.utils -from .common import SimConnection -class TestUtils(unittest.TestCase): # Check that transform_to_matrix calculates the right values +class TestUtils(unittest.TestCase): # Check that transform_to_matrix calculates the right values def test_transform_to_matrix(self): - transform = lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6)) - expectedMatrix = [[0.9913729386253347, 0.10427383718471564, -0.07941450396586013, 0.0], \ - [-0.0980843287345578, 0.9920992900156518, 0.07822060602635744, 0.0], \ - [0.08694343573875718, -0.0697564737441253, 0.9937680178757644, 0.0], \ - [1, 2, 3, 1.0]] + transform = lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6)) + expectedMatrix = [[0.9913729386253347, 0.10427383718471564, -0.07941450396586013, 0.0], + [-0.0980843287345578, 0.9920992900156518, 0.07822060602635744, 0.0], + [0.08694343573875718, -0.0697564737441253, 0.9937680178757644, 0.0], + [1, 2, 3, 1.0]] matrix = lgsvl.utils.transform_to_matrix(transform) for i in range(4): for j in range(4): - self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) - - def test_matrix_multiply(self): # Check that matrix_multiply calculates the right values - inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) - expectedMatrix = [[0.9656881042915112, 0.21236393599051254, -0.1494926216255657, 0.0], \ - [-0.18774677387638924, 0.9685769782741936, 0.1631250626244768, 0.0], \ - [0.1794369920860106, -0.12946117505974142, 0.9752139098799174, 0.0], \ - [2.0560345883724906, 3.8792029959836434, 6.058330761714148, 1.0]] + self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) + + def test_matrix_multiply(self): # Check that matrix_multiply calculates the right values + inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) + expectedMatrix = [[0.9656881042915112, 0.21236393599051254, -0.1494926216255657, 0.0], + [-0.18774677387638924, 0.9685769782741936, 0.1631250626244768, 0.0], + [0.1794369920860106, -0.12946117505974142, 0.9752139098799174, 0.0], + [2.0560345883724906, 3.8792029959836434, 6.058330761714148, 1.0]] + matrix = lgsvl.utils.matrix_multiply(inputMatrix, inputMatrix) for i in range(4): for j in range(4): - self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) - - def test_matrix_inverse(self): # Check that matrix_inverse calculates the right values - inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) - expectedMatrix = [[0.9913729386253347, -0.0980843287345578, 0.08694343573875718, 0.0], \ - [0.10427383718471564, 0.9920992900156518, -0.0697564737441253, 0.0], \ - [-0.07941450396586013, 0.07822060602635744, 0.9937680178757644, 0.0], \ - [-0.9616771010971856, -2.120776069375818, -2.9287345418778, 1.0]] + self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) + + def test_matrix_inverse(self): # Check that matrix_inverse calculates the right values + inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) + expectedMatrix = [[0.9913729386253347, -0.0980843287345578, 0.08694343573875718, 0.0], + [0.10427383718471564, 0.9920992900156518, -0.0697564737441253, 0.0], + [-0.07941450396586013, 0.07822060602635744, 0.9937680178757644, 0.0], + [-0.9616771010971856, -2.120776069375818, -2.9287345418778, 1.0]] + matrix = lgsvl.utils.matrix_inverse(inputMatrix) for i in range(4): for j in range(4): - self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) + self.assertAlmostEqual(matrix[i][j], expectedMatrix[i][j]) - def test_vector_multiply(self): # Check that vector_multiply calculates the right values - inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6))) - inputVector = lgsvl.Vector(10,20,30) + def test_vector_multiply(self): # Check that vector_multiply calculates the right values + inputMatrix = lgsvl.utils.transform_to_matrix(lgsvl.Transform(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6))) + inputVector = lgsvl.Vector(10, 20, 30) expectedVector = lgsvl.Vector(11.560345883724906, 20.792029959836434, 33.58330761714148) vector = lgsvl.utils.vector_multiply(inputVector, inputMatrix) @@ -56,6 +57,6 @@ def test_vector_multiply(self): # Check that vector_multiply calculates the righ self.assertAlmostEqual(vector.y, expectedVector.y) self.assertAlmostEqual(vector.z, expectedVector.z) - def test_vector_dot(self): # Check that vector_dot calculates the right values - result = lgsvl.utils.vector_dot(lgsvl.Vector(1,2,3), lgsvl.Vector(4,5,6)) + def test_vector_dot(self): # Check that vector_dot calculates the right values + result = lgsvl.utils.vector_dot(lgsvl.Vector(1, 2, 3), lgsvl.Vector(4, 5, 6)) self.assertAlmostEqual(result, 32)