From 96528b82ec28d9b0509e6fef24dfa2a42504808f Mon Sep 17 00:00:00 2001
From: Yuchao Feng <99387180+fyctime052@users.noreply.github.com>
Date: Fri, 26 Jan 2024 14:36:08 +0800
Subject: [PATCH] Add files via upload
---
scenario_runner/srunner/__init__.py | 0
.../0.9.x/OpenSCENARIO_Catalog.xsd | 39 +
.../0.9.x/OpenSCENARIO_TypeDefs.xsd | 1457 +++++++
.../0.9.x/OpenSCENARIO_v0.9.1.xsd | 218 +
.../0.9.x/migration0_9_1to1_0.xslt | 3679 +++++++++++++++++
.../srunner/openscenario/OpenSCENARIO.xsd | 1506 +++++++
.../srunner/scenarioconfigs/__init__.py | 0
.../openscenario_configuration.py | 381 ++
.../route_scenario_configuration.py | 50 +
.../scenarioconfigs/scenario_configuration.py | 86 +
.../srunner/scenariomanager/__init__.py | 0
.../scenariomanager/actorcontrols/__init__.py | 0
.../actorcontrols/actor_control.py | 154 +
.../actorcontrols/basic_control.py | 106 +
.../actorcontrols/external_control.py | 43 +
.../actorcontrols/npc_vehicle_control.py | 106 +
.../actorcontrols/pedestrian_control.py | 71 +
.../actorcontrols/simple_vehicle_control.py | 282 ++
.../vehicle_longitudinal_control.py | 69 +
.../scenariomanager/carla_data_provider.py | 816 ++++
.../srunner/scenariomanager/result_writer.py | 217 +
.../scenariomanager/scenario_manager.py | 231 ++
.../scenarioatomics/__init__.py | 0
.../scenarioatomics/atomic_behaviors.py | 2460 +++++++++++
.../scenarioatomics/atomic_criteria.py | 2048 +++++++++
.../atomic_trigger_conditions.py | 1233 ++++++
.../srunner/scenariomanager/timer.py | 158 +
.../srunner/scenariomanager/traffic_events.py | 84 +
.../srunner/scenariomanager/watchdog.py | 79 +
.../srunner/scenariomanager/weather_sim.py | 166 +
scenario_runner/srunner/scenarios/__init__.py | 0
.../srunner/scenarios/background_activity.py | 93 +
.../srunner/scenarios/basic_scenario.py | 308 ++
.../srunner/scenarios/change_lane.py | 181 +
.../srunner/scenarios/control_loss.py | 198 +
scenario_runner/srunner/scenarios/cut_in.py | 158 +
.../scenarios/follow_leading_vehicle.py | 325 ++
scenario_runner/srunner/scenarios/freeride.py | 68 +
.../scenarios/junction_crossing_route.py | 203 +
.../scenarios/maneuver_opposite_direction.py | 172 +
.../srunner/scenarios/master_scenario.py | 114 +
.../scenarios/no_signal_junction_crossing.py | 164 +
.../scenarios/object_crash_intersection.py | 606 +++
.../srunner/scenarios/object_crash_vehicle.py | 404 ++
.../srunner/scenarios/open_scenario.py | 455 ++
.../opposite_vehicle_taking_priority.py | 227 +
.../scenarios/other_leading_vehicle.py | 151 +
.../srunner/scenarios/route_scenario.py | 515 +++
.../signalized_junction_left_turn.py | 150 +
.../signalized_junction_right_turn.py | 162 +
scenario_runner/srunner/tools/__init__.py | 0
.../srunner/tools/openscenario_parser.py | 1061 +++++
.../srunner/tools/py_trees_port.py | 137 +
.../srunner/tools/route_manipulation.py | 159 +
scenario_runner/srunner/tools/route_parser.py | 325 ++
.../srunner/tools/scenario_helper.py | 510 +++
.../srunner/tools/scenario_parser.py | 124 +
.../utilities/code_check_and_formatting.sh | 18 +
58 files changed, 22727 insertions(+)
create mode 100644 scenario_runner/srunner/__init__.py
create mode 100644 scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_Catalog.xsd
create mode 100644 scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_TypeDefs.xsd
create mode 100644 scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_v0.9.1.xsd
create mode 100644 scenario_runner/srunner/openscenario/0.9.x/migration0_9_1to1_0.xslt
create mode 100644 scenario_runner/srunner/openscenario/OpenSCENARIO.xsd
create mode 100644 scenario_runner/srunner/scenarioconfigs/__init__.py
create mode 100644 scenario_runner/srunner/scenarioconfigs/openscenario_configuration.py
create mode 100644 scenario_runner/srunner/scenarioconfigs/route_scenario_configuration.py
create mode 100644 scenario_runner/srunner/scenarioconfigs/scenario_configuration.py
create mode 100644 scenario_runner/srunner/scenariomanager/__init__.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/__init__.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/actor_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/basic_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/external_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/pedestrian_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py
create mode 100644 scenario_runner/srunner/scenariomanager/carla_data_provider.py
create mode 100644 scenario_runner/srunner/scenariomanager/result_writer.py
create mode 100644 scenario_runner/srunner/scenariomanager/scenario_manager.py
create mode 100644 scenario_runner/srunner/scenariomanager/scenarioatomics/__init__.py
create mode 100644 scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py
create mode 100644 scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
create mode 100644 scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py
create mode 100644 scenario_runner/srunner/scenariomanager/timer.py
create mode 100644 scenario_runner/srunner/scenariomanager/traffic_events.py
create mode 100644 scenario_runner/srunner/scenariomanager/watchdog.py
create mode 100644 scenario_runner/srunner/scenariomanager/weather_sim.py
create mode 100644 scenario_runner/srunner/scenarios/__init__.py
create mode 100644 scenario_runner/srunner/scenarios/background_activity.py
create mode 100644 scenario_runner/srunner/scenarios/basic_scenario.py
create mode 100644 scenario_runner/srunner/scenarios/change_lane.py
create mode 100644 scenario_runner/srunner/scenarios/control_loss.py
create mode 100644 scenario_runner/srunner/scenarios/cut_in.py
create mode 100644 scenario_runner/srunner/scenarios/follow_leading_vehicle.py
create mode 100644 scenario_runner/srunner/scenarios/freeride.py
create mode 100644 scenario_runner/srunner/scenarios/junction_crossing_route.py
create mode 100644 scenario_runner/srunner/scenarios/maneuver_opposite_direction.py
create mode 100644 scenario_runner/srunner/scenarios/master_scenario.py
create mode 100644 scenario_runner/srunner/scenarios/no_signal_junction_crossing.py
create mode 100644 scenario_runner/srunner/scenarios/object_crash_intersection.py
create mode 100644 scenario_runner/srunner/scenarios/object_crash_vehicle.py
create mode 100644 scenario_runner/srunner/scenarios/open_scenario.py
create mode 100644 scenario_runner/srunner/scenarios/opposite_vehicle_taking_priority.py
create mode 100644 scenario_runner/srunner/scenarios/other_leading_vehicle.py
create mode 100644 scenario_runner/srunner/scenarios/route_scenario.py
create mode 100644 scenario_runner/srunner/scenarios/signalized_junction_left_turn.py
create mode 100644 scenario_runner/srunner/scenarios/signalized_junction_right_turn.py
create mode 100644 scenario_runner/srunner/tools/__init__.py
create mode 100644 scenario_runner/srunner/tools/openscenario_parser.py
create mode 100644 scenario_runner/srunner/tools/py_trees_port.py
create mode 100644 scenario_runner/srunner/tools/route_manipulation.py
create mode 100644 scenario_runner/srunner/tools/route_parser.py
create mode 100644 scenario_runner/srunner/tools/scenario_helper.py
create mode 100644 scenario_runner/srunner/tools/scenario_parser.py
create mode 100644 scenario_runner/srunner/utilities/code_check_and_formatting.sh
diff --git a/scenario_runner/srunner/__init__.py b/scenario_runner/srunner/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_Catalog.xsd b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_Catalog.xsd
new file mode 100644
index 0000000..2ba333c
--- /dev/null
+++ b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_Catalog.xsd
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+ XML Schema Definition for OpenSCENARIO Catalog XML files - Version Draft 0.9.1, (c)2017 by VIRES Simulationstechnologie GmbH, Germany
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_TypeDefs.xsd b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_TypeDefs.xsd
new file mode 100644
index 0000000..931d8cf
--- /dev/null
+++ b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_TypeDefs.xsd
@@ -0,0 +1,1457 @@
+
+
+
+
+
+
+
+ XML Schema Type Definitions for OpenSCENARIO XML files - Version Draft 0.9.1, (c)2017 by VIRES Simulationstechnologie GmbH, Germany
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_v0.9.1.xsd b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_v0.9.1.xsd
new file mode 100644
index 0000000..d892872
--- /dev/null
+++ b/scenario_runner/srunner/openscenario/0.9.x/OpenSCENARIO_v0.9.1.xsd
@@ -0,0 +1,218 @@
+
+
+
+
+
+
+
+
+
+ XML Schema Definition for OpenSCENARIO XML files - Version 0.9.1, (c)2017 by VIRES Simulationstechnologie GmbH, Germany
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenario_runner/srunner/openscenario/0.9.x/migration0_9_1to1_0.xslt b/scenario_runner/srunner/openscenario/0.9.x/migration0_9_1to1_0.xslt
new file mode 100644
index 0000000..54dbc7a
--- /dev/null
+++ b/scenario_runner/srunner/openscenario/0.9.x/migration0_9_1to1_0.xslt
@@ -0,0 +1,3679 @@
+
+
+
+
+
+
+
+ longitudinal
+ lateral
+ cartesianDistance
+
+
+
+
+
+ greaterThan
+ lessThan
+ equalTo
+
+
+
+
+
+ none
+ railing
+ barrier
+
+
+
+
+
+ endTransition
+ stopTransition
+ completeState
+
+
+
+
+
+ risingOrFalling
+ falling
+ rising
+
+
+
+
+
+ position
+ follow
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Review catalogs since driver catalog and pedestrian catalogs are merged into controller catalog
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: OSCTrajectory is restructured and connot be automatically migrated. Review XML
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Review catalogs since driver catalog and pedestrian catalogs are merged into controller catalog
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Review catalogs since driver catalog and pedestrian catalogs are merged into controller catalog
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Jam is desupported: removed during migration
+
+
+
+
+ WARNING: Script is desupported: removed during migration
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: OSCPrivateAction.Meeting is desupported: removed during migration
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ERROR: Act: Cancel and End are both defined: Version 1.0 only supports Stop Trigger. Invalid XML is produced and needs to be revisited.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Storyboard.Story.Act.Sequence.Actors.ByCondition: "anyEntity" is de-supported for version 1.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: OSCPrivateAction.Longitudinal.Speed or OSCPrivateAction.Lateral.LaneChange.Dynamics: Distance, time, rate must exclude each other in the original file. This results in a invalid output.
+
+
+
+
+ WARNING: OSCPrivateAction.Longitudinal.Speed or OSCPrivateAction.Lateral.LaneChange.Dynamics: There must be at least one of time, rate or distance in the original file. This results in a invalid output.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Review catalogs since driver catalog and pedestrian catalogs are merged into controller catalog
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: Event.priority: 'following' is desupported: removed during migration. This results in unvalifd XML code.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: OSCPrivateAction.Meeting is desupported: removed during migration: This may result in unvalid XML
+
+
+
+
+ WARNING: Jam is desupported: removed during migration: This may result in unvalid XML
+
+
+
+
+ WARNING: Script is desupported: removed during migration: This may result in unvalid XML
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Migration: Insert Controller here
+
+
+
+ ERROR: OSCTrafficDefinition.DriverDistribution.Driver cannot be migrated automatically and will result in unvalid XML output.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ WARNING: OSCTrajectory.Vertex: OSCTrajectory is restructured and connot be automatically migrated. Review XML.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/scenario_runner/srunner/openscenario/OpenSCENARIO.xsd b/scenario_runner/srunner/openscenario/OpenSCENARIO.xsd
new file mode 100644
index 0000000..9a96219
--- /dev/null
+++ b/scenario_runner/srunner/openscenario/OpenSCENARIO.xsd
@@ -0,0 +1,1506 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenario_runner/srunner/scenarioconfigs/__init__.py b/scenario_runner/srunner/scenarioconfigs/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/scenarioconfigs/openscenario_configuration.py b/scenario_runner/srunner/scenarioconfigs/openscenario_configuration.py
new file mode 100644
index 0000000..6934b0c
--- /dev/null
+++ b/scenario_runner/srunner/scenarioconfigs/openscenario_configuration.py
@@ -0,0 +1,381 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides the key configuration parameters for a scenario based on OpenSCENARIO
+"""
+
+import logging
+import os
+import xml.etree.ElementTree as ET
+
+import xmlschema
+
+import carla
+
+# pylint: disable=line-too-long
+from srunner.scenarioconfigs.scenario_configuration import ActorConfigurationData, ScenarioConfiguration
+# pylint: enable=line-too-long
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider # workaround
+from srunner.tools.openscenario_parser import OpenScenarioParser
+
+
+class OpenScenarioConfiguration(ScenarioConfiguration):
+
+ """
+ Limitations:
+ - Only one Story + Init is supported per Storyboard
+ """
+
+ def __init__(self, filename, client):
+
+ self.xml_tree = ET.parse(filename)
+ self._filename = filename
+
+ self._validate_openscenario_configuration()
+ self.client = client
+
+ self.catalogs = {}
+
+ self.other_actors = []
+ self.ego_vehicles = []
+ self.trigger_points = []
+ self.weather = carla.WeatherParameters()
+
+ self.storyboard = self.xml_tree.find("Storyboard")
+ self.story = self.storyboard.find("Story")
+ self.init = self.storyboard.find("Init")
+
+ logging.basicConfig()
+ self.logger = logging.getLogger("[SR:OpenScenarioConfiguration]")
+
+ self._global_parameters = {}
+
+ self._set_parameters()
+ self._parse_openscenario_configuration()
+
+ def _validate_openscenario_configuration(self):
+ """
+ Validate the given OpenSCENARIO config against the 0.9.1 XSD
+
+ Note: This will throw if the config is not valid. But this is fine here.
+ """
+ xsd_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../openscenario/OpenSCENARIO.xsd")
+ xsd = xmlschema.XMLSchema(xsd_file)
+ xsd.validate(self.xml_tree)
+
+ def _validate_openscenario_catalog_configuration(self, catalog_xml_tree):
+ """
+ Validate the given OpenSCENARIO catalog config against the 0.9.1 XSD
+
+ Note: This will throw if the catalog config is not valid. But this is fine here.
+ """
+ xsd_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../openscenario/OpenSCENARIO.xsd")
+ xsd = xmlschema.XMLSchema(xsd_file)
+ xsd.validate(catalog_xml_tree)
+
+ def _parse_openscenario_configuration(self):
+ """
+ Parse the given OpenSCENARIO config file, set and validate parameters
+ """
+ OpenScenarioParser.set_osc_filepath(os.path.dirname(self._filename))
+
+ self._check_version()
+ self._load_catalogs()
+ self._set_scenario_name()
+ self._set_carla_town()
+ self._set_actor_information()
+
+ self._validate_result()
+
+ def _check_version(self):
+ """
+ Ensure correct OpenSCENARIO version is used
+ """
+ header = self.xml_tree.find("FileHeader")
+ if not (header.attrib.get('revMajor') == "1" and header.attrib.get('revMinor') == "0"):
+ raise AttributeError("Only OpenSCENARIO 1.0 is supported")
+
+ def _load_catalogs(self):
+ """
+ Read Catalog xml files into dictionary for later use
+
+ NOTE: Catalogs must have distinct names, even across different types
+ """
+ catalogs = self.xml_tree.find("CatalogLocations")
+ if list(catalogs) is None:
+ return
+
+ catalog_types = ["Vehicle",
+ "Controller",
+ "Pedestrian",
+ "MiscObject",
+ "Environment",
+ "Maneuver",
+ "Trajectory",
+ "Route"]
+ for catalog_type in catalog_types:
+ catalog = catalogs.find(catalog_type + "Catalog")
+ if catalog is None:
+ continue
+
+ catalog_path = catalog.find("Directory").attrib.get('path') + "/" + catalog_type + "Catalog.xosc"
+ if not os.path.isabs(catalog_path) and "xosc" in self._filename:
+ catalog_path = os.path.dirname(os.path.abspath(self._filename)) + "/" + catalog_path
+
+ if not os.path.isfile(catalog_path):
+ self.logger.warning(" The %s path for the %s Catalog is invalid", catalog_path, catalog_type)
+ else:
+ xml_tree = ET.parse(catalog_path)
+ self._validate_openscenario_catalog_configuration(xml_tree)
+ catalog = xml_tree.find("Catalog")
+ catalog_name = catalog.attrib.get("name")
+ self.catalogs[catalog_name] = {}
+ for entry in catalog:
+ self.catalogs[catalog_name][entry.attrib.get("name")] = entry
+
+ def _set_scenario_name(self):
+ """
+ Extract the scenario name from the OpenSCENARIO header information
+ """
+ header = self.xml_tree.find("FileHeader")
+ self.name = header.attrib.get('description', 'Unknown')
+
+ if self.name.startswith("CARLA:"):
+ OpenScenarioParser.set_use_carla_coordinate_system()
+
+ def _set_carla_town(self):
+ """
+ Extract the CARLA town (level) from the RoadNetwork information from OpenSCENARIO
+
+ Note: The specification allows multiple Logics elements within the RoadNetwork element.
+ Hence, there can be multiple towns specified. We just use the _last_ one.
+ """
+ for logic in self.xml_tree.find("RoadNetwork").findall("LogicFile"):
+ self.town = logic.attrib.get('filepath', None)
+
+ if self.town is not None and ".xodr" in self.town:
+ if not os.path.isabs(self.town):
+ self.town = os.path.dirname(os.path.abspath(self._filename)) + "/" + self.town
+ if not os.path.exists(self.town):
+ raise AttributeError("The provided RoadNetwork '{}' does not exist".format(self.town))
+
+ # workaround for relative positions during init
+ world = self.client.get_world()
+ if world is None or world.get_map().name != self.town:
+ self.logger.warning(" Wrong OpenDRIVE map in use. Forcing reload of CARLA world")
+ if ".xodr" in self.town:
+ with open(self.town) as od_file:
+ data = od_file.read()
+ self.client.generate_opendrive_world(str(data))
+ else:
+ self.client.load_world(self.town)
+ world = self.client.get_world()
+ CarlaDataProvider.set_world(world)
+ world.wait_for_tick()
+ else:
+ CarlaDataProvider.set_world(world)
+
+ def _set_parameters(self):
+ """
+ Parse the complete scenario definition file, and replace all parameter references
+ with the actual values
+
+ Set _global_parameters.
+ """
+
+ self.xml_tree, self._global_parameters = OpenScenarioParser.set_parameters(self.xml_tree)
+
+ for elem in self.xml_tree.iter():
+ if elem.find('ParameterDeclarations') is not None:
+ elem, _ = OpenScenarioParser.set_parameters(elem)
+
+ OpenScenarioParser.set_global_parameters(self._global_parameters)
+
+ def _set_actor_information(self):
+ """
+ Extract all actors and their corresponding specification
+
+ NOTE: The rolename property has to be unique!
+ """
+ for entity in self.xml_tree.iter("Entities"):
+ for obj in entity.iter("ScenarioObject"):
+ rolename = obj.attrib.get('name', 'simulation')
+ args = dict()
+ for prop in obj.iter("Property"):
+ key = prop.get('name')
+ value = prop.get('value')
+ args[key] = value
+
+ for catalog_reference in obj.iter("CatalogReference"):
+ entry = OpenScenarioParser.get_catalog_entry(self.catalogs, catalog_reference)
+ if entry.tag == "Vehicle":
+ self._extract_vehicle_information(entry, rolename, entry, args)
+ elif entry.tag == "Pedestrian":
+ self._extract_pedestrian_information(entry, rolename, entry, args)
+ elif entry.tag == "MiscObject":
+ self._extract_misc_information(entry, rolename, entry, args)
+ else:
+ self.logger.error(
+ " A CatalogReference specifies a reference that is not an Entity. Skipping...")
+
+ for vehicle in obj.iter("Vehicle"):
+ self._extract_vehicle_information(obj, rolename, vehicle, args)
+
+ for pedestrian in obj.iter("Pedestrian"):
+ self._extract_pedestrian_information(obj, rolename, pedestrian, args)
+
+ for misc in obj.iter("MiscObject"):
+ self._extract_misc_information(obj, rolename, misc, args)
+
+ # Set transform for all actors
+ # This has to be done in a multi-stage loop to resolve relative position settings
+ all_actor_transforms_set = False
+ while not all_actor_transforms_set:
+ all_actor_transforms_set = True
+ for actor in self.other_actors + self.ego_vehicles:
+ if actor.transform is None:
+ try:
+ actor.transform = self._get_actor_transform(actor.rolename)
+ except AttributeError as e:
+ if "Object '" in str(e):
+ ref_actor_rolename = str(e).split('\'')[1]
+ for ref_actor in self.other_actors + self.ego_vehicles:
+ if ref_actor.rolename == ref_actor_rolename:
+ if ref_actor.transform is not None:
+ raise e
+ break
+ if actor.transform is None:
+ all_actor_transforms_set = False
+
+ def _extract_vehicle_information(self, obj, rolename, vehicle, args):
+ """
+ Helper function to _set_actor_information for getting vehicle information from XML tree
+ """
+ color = None
+ model = vehicle.attrib.get('name', "vehicle.*")
+ category = vehicle.attrib.get('vehicleCategory', "car")
+ ego_vehicle = False
+ for prop in obj.iter("Property"):
+ if prop.get('name', '') == 'type':
+ ego_vehicle = prop.get('value') == 'ego_vehicle'
+ if prop.get('name', '') == 'color':
+ color = prop.get('value')
+
+ speed = self._get_actor_speed(rolename)
+ new_actor = ActorConfigurationData(
+ model, None, rolename, speed, color=color, category=category, args=args)
+
+ if ego_vehicle:
+ self.ego_vehicles.append(new_actor)
+ else:
+ self.other_actors.append(new_actor)
+
+ def _extract_pedestrian_information(self, obj, rolename, pedestrian, args):
+ """
+ Helper function to _set_actor_information for getting pedestrian information from XML tree
+ """
+ model = pedestrian.attrib.get('model', "walker.*")
+
+ speed = self._get_actor_speed(rolename)
+ new_actor = ActorConfigurationData(model, None, rolename, speed, category="pedestrian", args=args)
+
+ self.other_actors.append(new_actor)
+
+ def _extract_misc_information(self, obj, rolename, misc, args):
+ """
+ Helper function to _set_actor_information for getting vehicle information from XML tree
+ """
+ category = misc.attrib.get('miscObjectCategory')
+ if category == "barrier":
+ model = "static.prop.streetbarrier"
+ elif category == "guardRail":
+ model = "static.prop.chainbarrier"
+ else:
+ model = misc.attrib.get('name')
+ new_actor = ActorConfigurationData(model, None, rolename, category="misc", args=args)
+
+ self.other_actors.append(new_actor)
+
+ def _get_actor_transform(self, actor_name):
+ """
+ Get the initial actor transform provided by the Init section
+
+ Note: - The OpenScenario specification allows multiple definitions. We use the _first_ one
+ - The OpenScenario specification allows different ways of specifying a position.
+ We currently support the specification with absolute world coordinates and the relative positions
+ RelativeWorld, RelativeObject and RelativeLane
+ - When using relative positions the relevant reference point (e.g. transform of another actor)
+ should be defined before!
+ """
+
+ actor_transform = carla.Transform()
+
+ actor_found = False
+
+ for private_action in self.init.iter("Private"):
+ if private_action.attrib.get('entityRef', None) == actor_name:
+ if actor_found:
+ # pylint: disable=line-too-long
+ self.logger.warning(
+ " Warning: The actor '%s' was already assigned an initial position. Overwriting pose!", actor_name)
+ # pylint: enable=line-too-long
+ actor_found = True
+ for position in private_action.iter('Position'):
+ transform = OpenScenarioParser.convert_position_to_transform(
+ position, actor_list=self.other_actors + self.ego_vehicles)
+ if transform:
+ actor_transform = transform
+
+ if not actor_found:
+ # pylint: disable=line-too-long
+ self.logger.warning(
+ " Warning: The actor '%s' was not assigned an initial position. Using (0,0,0)", actor_name)
+ # pylint: enable=line-too-long
+
+ return actor_transform
+
+ def _get_actor_speed(self, actor_name):
+ """
+ Get the initial actor speed provided by the Init section
+ """
+ actor_speed = 0
+ actor_found = False
+
+ for private_action in self.init.iter("Private"):
+ if private_action.attrib.get('entityRef', None) == actor_name:
+ if actor_found:
+ # pylint: disable=line-too-long
+ self.logger.warning(
+ " Warning: The actor '%s' was already assigned an initial speed. Overwriting inital speed!", actor_name)
+ # pylint: enable=line-too-long
+ actor_found = True
+
+ for longitudinal_action in private_action.iter('LongitudinalAction'):
+ for speed in longitudinal_action.iter('SpeedAction'):
+ for target in speed.iter('SpeedActionTarget'):
+ for absolute in target.iter('AbsoluteTargetSpeed'):
+ speed = float(absolute.attrib.get('value', 0))
+ if speed >= 0:
+ actor_speed = speed
+ else:
+ raise AttributeError(
+ "Warning: Speed value of actor {} must be positive. Speed set to 0.".format(actor_name)) # pylint: disable=line-too-long
+ return actor_speed
+
+ def _validate_result(self):
+ """
+ Check that the current scenario configuration is valid
+ """
+ if not self.name:
+ raise AttributeError("No scenario name found")
+
+ if not self.town:
+ raise AttributeError("CARLA level not defined")
+
+ if not self.ego_vehicles:
+ self.logger.warning(" No ego vehicles defined in scenario")
diff --git a/scenario_runner/srunner/scenarioconfigs/route_scenario_configuration.py b/scenario_runner/srunner/scenarioconfigs/route_scenario_configuration.py
new file mode 100644
index 0000000..63f4318
--- /dev/null
+++ b/scenario_runner/srunner/scenarioconfigs/route_scenario_configuration.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides the key configuration parameters for a route-based scenario
+"""
+
+import carla
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration
+
+
+class RouteConfiguration(object):
+
+ """
+ This class provides the basic configuration for a route
+ """
+
+ def __init__(self, route=None):
+ self.data = route
+
+ def parse_xml(self, node):
+ """
+ Parse route config XML
+ """
+ self.data = []
+
+ for waypoint in node.iter("waypoint"):
+ x = float(waypoint.attrib.get('x', 0))
+ y = float(waypoint.attrib.get('y', 0))
+ z = float(waypoint.attrib.get('z', 0))
+ c = waypoint.attrib.get('connection', '')
+ connection = RoadOption[c.split('.')[1]]
+
+ self.data.append((carla.Location(x, y, z), connection))
+
+
+class RouteScenarioConfiguration(ScenarioConfiguration):
+
+ """
+ Basic configuration of a RouteScenario
+ """
+
+ trajectory = None
+ scenario_file = None
diff --git a/scenario_runner/srunner/scenarioconfigs/scenario_configuration.py b/scenario_runner/srunner/scenarioconfigs/scenario_configuration.py
new file mode 100644
index 0000000..388a7d2
--- /dev/null
+++ b/scenario_runner/srunner/scenarioconfigs/scenario_configuration.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides the key configuration parameters for an XML-based scenario
+"""
+
+import carla
+
+
+class ActorConfigurationData(object):
+
+ """
+ This is a configuration base class to hold model and transform attributes
+ """
+
+ def __init__(self, model, transform, rolename='other', speed=0, autopilot=False,
+ random=False, color=None, category="car", args=None):
+ self.model = model
+ self.rolename = rolename
+ self.transform = transform
+ self.speed = speed
+ self.autopilot = autopilot
+ self.random_location = random
+ self.color = color
+ self.category = category
+ self.args = args
+
+ @staticmethod
+ def parse_from_node(node, rolename):
+ """
+ static method to initialize an ActorConfigurationData from a given ET tree
+ """
+
+ model = node.attrib.get('model', 'vehicle.*')
+
+ pos_x = float(node.attrib.get('x', 0))
+ pos_y = float(node.attrib.get('y', 0))
+ pos_z = float(node.attrib.get('z', 0))
+ yaw = float(node.attrib.get('yaw', 0))
+
+ transform = carla.Transform(carla.Location(x=pos_x, y=pos_y, z=pos_z), carla.Rotation(yaw=yaw))
+
+ rolename = node.attrib.get('rolename', rolename)
+
+ speed = node.attrib.get('speed', 0)
+
+ autopilot = False
+ if 'autopilot' in node.keys():
+ autopilot = True
+
+ random_location = False
+ if 'random_location' in node.keys():
+ random_location = True
+
+ color = node.attrib.get('color', None)
+
+ return ActorConfigurationData(model, transform, rolename, speed, autopilot, random_location, color)
+
+
+class ScenarioConfiguration(object):
+
+ """
+ This class provides a basic scenario configuration incl.:
+ - configurations for all actors
+ - town, where the scenario should be executed
+ - name of the scenario (e.g. ControlLoss_1)
+ - type is the class of scenario (e.g. ControlLoss)
+ """
+
+ trigger_points = []
+ ego_vehicles = []
+ other_actors = []
+ town = None
+ name = None
+ type = None
+ route = None
+ agent = None
+ weather = carla.WeatherParameters()
+ friction = None
+ subtype = None
+ route_var_name = None
diff --git a/scenario_runner/srunner/scenariomanager/__init__.py b/scenario_runner/srunner/scenariomanager/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/__init__.py b/scenario_runner/srunner/scenariomanager/actorcontrols/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/actor_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/actor_control.py
new file mode 100644
index 0000000..d4aa5c5
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/actor_control.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides a wrapper to access/use user-defined actor
+controls for example to realize OpenSCENARIO controllers.
+
+At the moment only controls implemented in Python are supported.
+
+A user must not modify this module.
+"""
+
+import importlib
+import os
+import sys
+
+import carla
+
+from srunner.scenariomanager.actorcontrols.external_control import ExternalControl
+from srunner.scenariomanager.actorcontrols.npc_vehicle_control import NpcVehicleControl
+from srunner.scenariomanager.actorcontrols.pedestrian_control import PedestrianControl
+
+
+class ActorControl(object):
+
+ """
+ This class provides a wrapper (access mechanism) for user-defined actor controls.
+ The controllers are loaded via importlib. Therefore, the module name of the controller
+ has to match the control class name (e.g. my_own_control.py and MyOwnControl()).
+
+ At the moment only controllers implemented in Python are supported, or controllers that
+ are completely implemented outside of ScenarioRunner (see ExternalControl).
+
+ This wrapper is for example used to realize the OpenSCENARIO controllers.
+
+ Note:
+ If no controllers are provided in OpenSCENARIO a default controller for vehicles and
+ pedestrians is instantiated. For vehicles the NpcVehicleControl is used, for pedestrians
+ it is the PedestrianControl.
+
+ Args:
+ actor (carla.Actor): Actor that should be controlled by the controller.
+ control_py_module (string): Fully qualified path to the controller python module.
+ args (dict): A dictionary containing all parameters of the controller as (key, value) pairs.
+
+ Attributes:
+ control_instance: Instance of the user-defined controller.
+ _last_longitudinal_command: Timestamp of the last issued longitudinal control command (e.g. target speed).
+ Defaults to None. Used to avoid that 2 longitudinal control commands are issued at the same time.
+ _last_waypoint_command: Timestamp of the last issued waypoint control command.
+ Defaults to None. Used to avoid that 2 waypoint control commands are issued at the same time.
+ """
+
+ control_instance = None
+
+ _last_longitudinal_command = None
+ _last_waypoint_command = None
+
+ def __init__(self, actor, control_py_module, args):
+
+ # use importlib to import the control module
+ if not control_py_module:
+ if isinstance(actor, carla.Walker):
+ self.control_instance = PedestrianControl(actor)
+ elif isinstance(actor, carla.Vehicle):
+ self.control_instance = NpcVehicleControl(actor)
+ else:
+ # use ExternalControl for all misc objects to handle all actors the same way
+ self.control_instance = ExternalControl(actor)
+ else:
+ if ".py" in control_py_module:
+ module_name = os.path.basename(control_py_module).split('.')[0]
+ sys.path.append(os.path.dirname(control_py_module))
+ module_control = importlib.import_module(module_name)
+ control_class_name = module_control.__name__.title().replace('_', '')
+ else:
+ sys.path.append(os.path.dirname(__file__))
+ module_control = importlib.import_module(control_py_module)
+ control_class_name = control_py_module.split('.')[-1].title().replace('_', '')
+
+ self.control_instance = getattr(module_control, control_class_name)(actor, args)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ self.control_instance.reset()
+
+ def update_target_speed(self, target_speed, start_time=None):
+ """
+ Update the actor's target speed.
+
+ Args:
+ target_speed (float): New target speed [m/s].
+ start_time (float): Start time of the new "maneuver" [s].
+ """
+ self.control_instance.update_target_speed(target_speed)
+ if start_time:
+ self._last_longitudinal_command = start_time
+
+ def update_waypoints(self, waypoints, start_time=None):
+ """
+ Update the actor's waypoints
+
+ Args:
+ waypoints (List of carla.Transform): List of new waypoints.
+ start_time (float): Start time of the new "maneuver" [s].
+ """
+ self.control_instance.update_waypoints(waypoints)
+ if start_time:
+ self._last_waypoint_command = start_time
+
+ def check_reached_waypoint_goal(self):
+ """
+ Check if the actor reached the end of the waypoint list
+
+ returns:
+ True if the end was reached, False otherwise.
+ """
+ return self.control_instance.check_reached_waypoint_goal()
+
+ def get_last_longitudinal_command(self):
+ """
+ Get timestamp of the last issued longitudinal control command (target_speed)
+
+ returns:
+ Timestamp of last longitudinal control command
+ """
+ return self._last_longitudinal_command
+
+ def get_last_waypoint_command(self):
+ """
+ Get timestamp of the last issued waypoint control command
+
+ returns:
+ Timestamp of last waypoint control command
+ """
+ return self._last_waypoint_command
+
+ def set_init_speed(self):
+ """
+ Update the actor's initial speed setting
+ """
+ self.control_instance.set_init_speed()
+
+ def run_step(self):
+ """
+ Execute on tick of the controller's control loop
+ """
+ self.control_instance.run_step()
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/basic_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/basic_control.py
new file mode 100644
index 0000000..b2ecb94
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/basic_control.py
@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides the base class for user-defined actor
+controllers. All user-defined controls must be derived from
+this class.
+
+A user must not modify the module.
+"""
+
+
+class BasicControl(object):
+
+ """
+ This class is the base class for user-defined actor controllers
+ All user-defined agents must be derived from this class.
+
+ Args:
+ actor (carla.Actor): Actor that should be controlled by the controller.
+
+ Attributes:
+ _actor (carla.Actor): Controlled actor.
+ Defaults to None.
+ _target_speed (float): Logitudinal target speed of the controller.
+ Defaults to 0.
+ _init_speed (float): Initial longitudinal speed of the controller.
+ Defaults to 0.
+ _waypoints (list of carla.Transform): List of target waypoints the actor
+ should travel along. A waypoint here is of type carla.Transform!
+ Defaults to [].
+ _waypoints_updated (boolean):
+ Defaults to False.
+ _reached_goal (boolean):
+ Defaults to False.
+ """
+
+ _actor = None
+ _waypoints = []
+ _waypoints_updated = False
+ _target_speed = 0
+ _reached_goal = False
+ _init_speed = False
+
+ def __init__(self, actor):
+ """
+ Initialize the actor
+ """
+ self._actor = actor
+
+ def update_target_speed(self, speed):
+ """
+ Update the actor's target speed and set _init_speed to False.
+
+ Args:
+ speed (float): New target speed [m/s].
+ """
+ self._target_speed = speed
+ self._init_speed = False
+
+ def update_waypoints(self, waypoints, start_time=None):
+ """
+ Update the actor's waypoints
+
+ Args:
+ waypoints (List of carla.Transform): List of new waypoints.
+ """
+ self._waypoints = waypoints
+ self._waypoints_updated = True
+
+ def set_init_speed(self):
+ """
+ Set _init_speed to True
+ """
+ self._init_speed = True
+
+ def check_reached_waypoint_goal(self):
+ """
+ Check if the actor reached the end of the waypoint list
+
+ returns:
+ True if the end was reached, False otherwise.
+ """
+ return self._reached_goal
+
+ def reset(self):
+ """
+ Pure virtual function to reset the controller. This should be implemented
+ in the user-defined agent implementation.
+ """
+ raise NotImplementedError(
+ "This function must be re-implemented by the user-defined actor control."
+ "If this error becomes visible the class hierarchy is somehow broken")
+
+ def run_step(self):
+ """
+ Pure virtual function to run one step of the controllers's control loop.
+ This should be implemented in the user-defined agent implementation.
+ """
+ raise NotImplementedError(
+ "This function must be re-implemented by the user-defined actor control."
+ "If this error becomes visible the class hierarchy is somehow broken")
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/external_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/external_control.py
new file mode 100644
index 0000000..694f2ff
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/external_control.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides an example controller for actors, that use an external
+software for longitudinal and lateral control command calculation.
+Examples for external controls are: Autoware, CARLA manual_control, etc.
+
+This module is not intended for modification.
+"""
+
+from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+
+
+class ExternalControl(BasicControl):
+
+ """
+ Actor control class for actors, with externally implemented longitudinal and
+ lateral controlers (e.g. Autoware).
+
+ Args:
+ actor (carla.Actor): Actor that should be controlled by the agent.
+ """
+
+ def __init__(self, actor, args=None):
+ super(ExternalControl, self).__init__(actor)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ if self._actor and self._actor.is_alive:
+ self._actor = None
+
+ def run_step(self):
+ """
+ The control loop and setting the actor controls is implemented externally.
+ """
+ pass
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py
new file mode 100644
index 0000000..3d74794
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py
@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides an example control for vehicles
+"""
+
+import math
+
+import carla
+from agents.navigation.basic_agent import LocalPlanner
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+
+
+class NpcVehicleControl(BasicControl):
+
+ """
+ Controller class for vehicles derived from BasicControl.
+
+ The controller makes use of the LocalPlanner implemented in CARLA.
+
+ Args:
+ actor (carla.Actor): Vehicle actor that should be controlled.
+ """
+
+ _args = {'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05}
+
+ def __init__(self, actor, args=None):
+ super(NpcVehicleControl, self).__init__(actor)
+
+ self._local_planner = LocalPlanner( # pylint: disable=undefined-variable
+ self._actor, opt_dict={
+ 'target_speed': self._target_speed * 3.6,
+ 'lateral_control_dict': self._args})
+
+ if self._waypoints:
+ self._update_plan()
+
+ def _update_plan(self):
+ """
+ Update the plan (waypoint list) of the LocalPlanner
+ """
+ plan = []
+ for transform in self._waypoints:
+ waypoint = CarlaDataProvider.get_map().get_waypoint(
+ transform.location, project_to_road=True, lane_type=carla.LaneType.Any)
+ plan.append((waypoint, RoadOption.LANEFOLLOW))
+ self._local_planner.set_global_plan(plan)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ if self._actor and self._actor.is_alive:
+ if self._local_planner:
+ self._local_planner.reset_vehicle()
+ self._local_planner = None
+ self._actor = None
+
+ def run_step(self):
+ """
+ Execute on tick of the controller's control loop
+
+ If _waypoints are provided, the vehicle moves towards the next waypoint
+ with the given _target_speed, until reaching the final waypoint. Upon reaching
+ the final waypoint, _reached_goal is set to True.
+
+ If _waypoints is empty, the vehicle moves in its current direction with
+ the given _target_speed.
+
+ If _init_speed is True, the control command is post-processed to ensure that
+ the initial actor velocity is maintained independent of physics.
+ """
+ self._reached_goal = False
+
+ if self._waypoints_updated:
+ self._waypoints_updated = False
+ self._update_plan()
+
+ target_speed = self._target_speed
+ self._local_planner.set_speed(target_speed * 3.6)
+ control = self._local_planner.run_step(debug=False)
+
+ # Check if the actor reached the end of the plan
+ if self._local_planner.done():
+ self._reached_goal = True
+
+ self._actor.apply_control(control)
+
+ if self._init_speed:
+ current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2)
+
+ # If _init_speed is set, and the PID controller is not yet up to the point to take over,
+ # we manually set the vehicle to drive with the correct velocity
+ if abs(target_speed - current_speed) > 3:
+ yaw = self._actor.get_transform().rotation.yaw * (math.pi / 180)
+ vx = math.cos(yaw) * target_speed
+ vy = math.sin(yaw) * target_speed
+ self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/pedestrian_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/pedestrian_control.py
new file mode 100644
index 0000000..4cce0f6
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/pedestrian_control.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides an example control for pedestrians
+"""
+
+import math
+
+import carla
+
+from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+
+
+class PedestrianControl(BasicControl):
+
+ """
+ Controller class for pedestrians derived from BasicControl.
+
+ Args:
+ actor (carla.Actor): Pedestrian actor that should be controlled.
+ """
+
+ def __init__(self, actor, args=None):
+ if not isinstance(actor, carla.Walker):
+ raise RuntimeError("PedestrianControl: The to be controlled actor is not a pedestrian")
+
+ super(PedestrianControl, self).__init__(actor)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ if self._actor and self._actor.is_alive:
+ self._actor = None
+
+ def run_step(self):
+ """
+ Execute on tick of the controller's control loop
+
+ If _waypoints are provided, the pedestrian moves towards the next waypoint
+ with the given _target_speed, until reaching the final waypoint. Upon reaching
+ the final waypoint, _reached_goal is set to True.
+
+ If _waypoints is empty, the pedestrians moves in its current direction with
+ the given _target_speed.
+ """
+ if not self._actor or not self._actor.is_alive:
+ return
+
+ control = self._actor.get_control()
+ control.speed = self._target_speed
+
+ if self._waypoints:
+ self._reached_goal = False
+ location = self._waypoints[0].location
+ direction = location - self._actor.get_location()
+ direction_norm = math.sqrt(direction.x**2 + direction.y**2)
+ control.direction = direction / direction_norm
+ self._actor.apply_control(control)
+ if direction_norm < 1.0:
+ self._waypoints = self._waypoints[1:]
+ if not self._waypoints:
+ self._reached_goal = True
+ else:
+ control.direction = self._actor.get_transform().rotation.get_forward_vector()
+ self._actor.apply_control(control)
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py
new file mode 100644
index 0000000..9358727
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/simple_vehicle_control.py
@@ -0,0 +1,282 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides an example control for vehicles which
+does not use CARLA's vehicle engine.
+
+Limitations:
+- Does not respect any traffic regulation: speed limit, traffic light, priorities, etc.
+- Can only consider obstacles in forward facing reaching (i.e. in tight corners obstacles may be ignored).
+"""
+
+from distutils.util import strtobool
+import math
+import cv2
+import numpy as np
+
+import carla
+
+from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import GameTime
+
+
+class SimpleVehicleControl(BasicControl):
+
+ """
+ Controller class for vehicles derived from BasicControl.
+
+ The controller directly sets velocities in CARLA, therefore bypassing
+ CARLA's vehicle engine. This allows a very precise speed control, but comes
+ with limitations during cornering.
+
+ In addition, the controller can consider blocking obstacles, which are
+ classified as dynamic (i.e. vehicles, bikes, pedestrians). Activation of this
+ features is controlled by passing proper arguments to the class constructor.
+ The collision detection uses CARLA's obstacle sensor (sensor.other.obstacle),
+ which checks for obstacles in the direct forward channel of the vehicle, i.e.
+ there are limitation with sideways obstacles and while cornering.
+
+ Args:
+ actor (carla.Actor): Vehicle actor that should be controlled.
+ args (dictionary): Dictonary of (key, value) arguments to be used by the controller.
+ May include: (consider_obstacles, true/false) - Enable consideration of obstacles
+ (proximity_threshold, distance) - Distance in front of actor in which
+ obstacles are considered
+ (attach_camera, true/false) - Attach OpenCV display to actor
+ (useful for debugging)
+
+ Attributes:
+
+ _generated_waypoint_list (list of carla.Transform): List of target waypoints the actor
+ should travel along. A waypoint here is of type carla.Transform!
+ Defaults to [].
+ _last_update (float): Last time step the update function (tick()) was called.
+ Defaults to None.
+ _consider_obstacles (boolean): Enable/Disable consideration of obstacles
+ Defaults to False.
+ _proximity_threshold (float): Distance in front of actor in which obstacles are considered
+ Defaults to infinity.
+ _cv_image (CV Image): Contains the OpenCV image, in case a debug camera is attached to the actor
+ Defaults to None.
+ _camera (sensor.camera.rgb): Debug camera attached to actor
+ Defaults to None.
+ _obstacle_sensor (sensor.other.obstacle): Obstacle sensor attached to actor
+ Defaults to None.
+ _obstacle_distance (float): Distance of the closest obstacle returned by the obstacle sensor
+ Defaults to infinity.
+ _obstacle_actor (carla.Actor): Closest obstacle returned by the obstacle sensor
+ Defaults to None.
+ """
+
+ def __init__(self, actor, args=None):
+ super(SimpleVehicleControl, self).__init__(actor)
+ self._generated_waypoint_list = []
+ self._last_update = None
+ self._consider_obstacles = False
+ self._proximity_threshold = float('inf')
+
+ self._cv_image = None
+ self._camera = None
+ self._obstacle_sensor = None
+ self._obstacle_distance = float('inf')
+ self._obstacle_actor = None
+
+ if args and 'consider_obstacles' in args and strtobool(args['consider_obstacles']):
+ self._consider_obstacles = strtobool(args['consider_obstacles'])
+ bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.other.obstacle')
+ bp.set_attribute('distance', '250')
+ if args and 'proximity_threshold' in args:
+ self._proximity_threshold = float(args['proximity_threshold'])
+ bp.set_attribute('distance', str(max(float(args['proximity_threshold']), 250)))
+ bp.set_attribute('hit_radius', '1')
+ bp.set_attribute('only_dynamics', 'True')
+ self._obstacle_sensor = CarlaDataProvider.get_world().spawn_actor(
+ bp, carla.Transform(carla.Location(x=self._actor.bounding_box.extent.x, z=1.0)), attach_to=self._actor)
+ self._obstacle_sensor.listen(lambda event: self._on_obstacle(event)) # pylint: disable=unnecessary-lambda
+
+ if args and 'attach_camera' in args and strtobool(args['attach_camera']):
+ bp = CarlaDataProvider.get_world().get_blueprint_library().find('sensor.camera.rgb')
+ self._camera = CarlaDataProvider.get_world().spawn_actor(bp, carla.Transform(
+ carla.Location(x=0.0, z=30.0), carla.Rotation(pitch=-60)), attach_to=self._actor)
+ self._camera.listen(lambda image: self._on_camera_update(image)) # pylint: disable=unnecessary-lambda
+
+ def _on_obstacle(self, event):
+ """
+ Callback for the obstacle sensor
+
+ Sets _obstacle_distance and _obstacle_actor according to the closest obstacle
+ found by the sensor.
+ """
+ if not event:
+ return
+ self._obstacle_distance = event.distance
+ self._obstacle_actor = event.other_actor
+
+ def _on_camera_update(self, image):
+ """
+ Callback for the camera sensor
+
+ Sets the OpenCV image (_cv_image). Requires conversion from BGRA to RGB.
+ """
+ if not image:
+ return
+
+ image_data = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
+ np_image = np.reshape(image_data, (image.height, image.width, 4))
+ np_image = np_image[:, :, :3]
+ np_image = np_image[:, :, ::-1]
+ self._cv_image = cv2.cvtColor(np_image, cv2.COLOR_BGR2RGB)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ if self._camera:
+ self._camera.destroy()
+ self._camera = None
+ if self._obstacle_sensor:
+ self._obstacle_sensor.destroy()
+ self._obstacle_sensor = None
+ if self._actor and self._actor.is_alive:
+ self._actor = None
+
+ def run_step(self):
+ """
+ Execute on tick of the controller's control loop
+
+ If _waypoints are provided, the vehicle moves towards the next waypoint
+ with the given _target_speed, until reaching the final waypoint. Upon reaching
+ the final waypoint, _reached_goal is set to True.
+
+ If _waypoints is empty, the vehicle moves in its current direction with
+ the given _target_speed.
+
+ If _consider_obstacles is true, the speed is adapted according to the closest
+ obstacle in front of the actor, if it is within the _proximity_threshold distance.
+ """
+
+ if self._cv_image is not None:
+ cv2.imshow("", self._cv_image)
+ cv2.waitKey(1)
+
+ if self._reached_goal:
+ # Reached the goal, so stop
+ velocity = carla.Vector3D(0, 0, 0)
+ self._actor.set_target_velocity(velocity)
+ return
+
+ self._reached_goal = False
+
+ if not self._waypoints:
+ # No waypoints are provided, so we have to create a list of waypoints internally
+ # get next waypoints from map, to avoid leaving the road
+ self._reached_goal = False
+
+ map_wp = None
+ if not self._generated_waypoint_list:
+ map_wp = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor))
+ else:
+ map_wp = CarlaDataProvider.get_map().get_waypoint(self._generated_waypoint_list[-1].location)
+ while len(self._generated_waypoint_list) < 50:
+ map_wps = map_wp.next(3.0)
+ if map_wps:
+ self._generated_waypoint_list.append(map_wps[0].transform)
+ map_wp = map_wps[0]
+ else:
+ break
+
+ direction_norm = self._set_new_velocity(self._generated_waypoint_list[0].location)
+ if direction_norm < 2.0:
+ self._generated_waypoint_list = self._generated_waypoint_list[1:]
+ else:
+ # When changing from "free" driving without pre-defined waypoints to a defined route with waypoints
+ # it may happen that the first few waypoints are too close to the ego vehicle for obtaining a
+ # reasonable control command. Therefore, we drop these waypoints first.
+ while self._waypoints and self._waypoints[0].location.distance(self._actor.get_location()) < 0.5:
+ self._waypoints = self._waypoints[1:]
+
+ self._reached_goal = False
+ direction_norm = self._set_new_velocity(self._waypoints[0].location)
+ if direction_norm < 4.0:
+ self._waypoints = self._waypoints[1:]
+ if not self._waypoints:
+ self._reached_goal = True
+
+ def _set_new_velocity(self, next_location):
+ """
+ Calculate and set the new actor veloctiy given the current actor
+ location and the _next_location_
+
+ If _consider_obstacles is true, the speed is adapted according to the closest
+ obstacle in front of the actor, if it is within the _proximity_threshold distance.
+
+ Args:
+ next_location (carla.Location): Next target location of the actor
+
+ returns:
+ direction (carla.Vector3D): Length of direction vector of the actor
+ """
+
+ current_time = GameTime.get_time()
+ target_speed = self._target_speed
+
+ if not self._last_update:
+ self._last_update = current_time
+
+ if self._consider_obstacles:
+ # If distance is less than the proximity threshold, adapt velocity
+ if self._obstacle_distance < self._proximity_threshold:
+ distance = max(self._obstacle_distance, 0)
+ if distance > 0:
+ current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2)
+ current_speed_other = math.sqrt(
+ self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2)
+ if current_speed_other < current_speed:
+ acceleration = -0.5 * (current_speed - current_speed_other)**2 / distance
+ target_speed = max(acceleration * (current_time - self._last_update) + current_speed, 0)
+ else:
+ target_speed = 0
+
+ # set new linear velocity
+ velocity = carla.Vector3D(0, 0, 0)
+ direction = next_location - CarlaDataProvider.get_location(self._actor)
+ direction_norm = math.sqrt(direction.x**2 + direction.y**2)
+ velocity.x = direction.x / direction_norm * target_speed
+ velocity.y = direction.y / direction_norm * target_speed
+
+ self._actor.set_target_velocity(velocity)
+
+ # set new angular velocity
+ current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
+ # When we have a waypoint list, use the direction between the waypoints to calculate the heading (change)
+ # otherwise use the waypoint heading directly
+ if self._waypoints:
+ delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw
+ else:
+ new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw
+ delta_yaw = new_yaw - current_yaw
+
+ if math.fabs(delta_yaw) > 360:
+ delta_yaw = delta_yaw % 360
+
+ if delta_yaw > 180:
+ delta_yaw = delta_yaw - 360
+ elif delta_yaw < -180:
+ delta_yaw = delta_yaw + 360
+
+ angular_velocity = carla.Vector3D(0, 0, 0)
+ if target_speed == 0:
+ angular_velocity.z = 0
+ else:
+ angular_velocity.z = delta_yaw / (direction_norm / target_speed)
+ self._actor.set_target_angular_velocity(angular_velocity)
+
+ self._last_update = current_time
+
+ return direction_norm
diff --git a/scenario_runner/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py b/scenario_runner/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py
new file mode 100644
index 0000000..9d6b9ee
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/actorcontrols/vehicle_longitudinal_control.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides an example longitudinal control for vehicles
+"""
+
+import math
+
+import carla
+
+from srunner.scenariomanager.actorcontrols.basic_control import BasicControl
+
+
+class VehicleLongitudinalControl(BasicControl):
+
+ """
+ Controller class for vehicles derived from BasicControl.
+
+ The controller only controls the throttle of a vehicle, but not the steering.
+
+ Args:
+ actor (carla.Actor): Vehicle actor that should be controlled.
+ """
+
+ def __init__(self, actor, args=None):
+ super(VehicleLongitudinalControl, self).__init__(actor)
+
+ def reset(self):
+ """
+ Reset the controller
+ """
+ if self._actor and self._actor.is_alive:
+ self._actor = None
+
+ def run_step(self):
+ """
+ Execute on tick of the controller's control loop
+
+ The control loop is very simplistic:
+ If the actor speed is below the _target_speed, set throttle to 1.0,
+ otherwise, set throttle to 0.0
+ Note, that this is a longitudinal controller only.
+
+ If _init_speed is True, the control command is post-processed to ensure that
+ the initial actor velocity is maintained independent of physics.
+ """
+
+ control = self._actor.get_control()
+
+ velocity = self._actor.get_velocity()
+ current_speed = math.sqrt(velocity.x**2 + velocity.y**2)
+ if current_speed < self._target_speed:
+ control.throttle = 1.0
+ else:
+ control.throttle = 0.0
+
+ self._actor.apply_control(control)
+
+ if self._init_speed:
+ if abs(self._target_speed - current_speed) > 3:
+ yaw = self._actor.get_transform().rotation.yaw * (math.pi / 180)
+ vx = math.cos(yaw) * self._target_speed
+ vy = math.sin(yaw) * self._target_speed
+ self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
diff --git a/scenario_runner/srunner/scenariomanager/carla_data_provider.py b/scenario_runner/srunner/scenariomanager/carla_data_provider.py
new file mode 100644
index 0000000..040c6aa
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/carla_data_provider.py
@@ -0,0 +1,816 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides all frequently used data from CARLA via
+local buffers to avoid blocking calls to CARLA
+"""
+
+from __future__ import print_function
+
+import math
+import re
+import numpy.random as random
+from six import iteritems
+
+import carla
+
+
+def calculate_velocity(actor):
+ """
+ Method to calculate the velocity of a actor
+ """
+ velocity_squared = actor.get_velocity().x**2
+ velocity_squared += actor.get_velocity().y**2
+ return math.sqrt(velocity_squared)
+
+
+class CarlaDataProvider(object): # pylint: disable=too-many-public-methods
+
+ """
+ This class provides access to various data of all registered actors
+ It buffers the data and updates it on every CARLA tick
+
+ Currently available data:
+ - Absolute velocity
+ - Location
+ - Transform
+
+ Potential additions:
+ - Acceleration
+
+ In addition it provides access to the map and the transform of all traffic lights
+ """
+
+ _actor_velocity_map = dict()
+ _actor_location_map = dict()
+ _actor_transform_map = dict()
+ _traffic_light_map = dict()
+ _carla_actor_pool = dict()
+ _client = None
+ _world = None
+ _map = None
+ _sync_flag = False
+ _spawn_points = None
+ _spawn_index = 0
+ _blueprint_library = None
+ _ego_vehicle_route = None
+ _traffic_manager_port = 8000
+
+ _ego_vehicle = None
+ _rng = random.RandomState(2000)
+
+ @staticmethod
+ def register_actor(actor):
+ """
+ Add new actor to dictionaries
+ If actor already exists, throw an exception
+ """
+ if actor in CarlaDataProvider._actor_velocity_map:
+ raise KeyError(
+ "Vehicle '{}' already registered. Cannot register twice!".format(actor.id))
+ else:
+ CarlaDataProvider._actor_velocity_map[actor] = 0.0
+
+ if actor in CarlaDataProvider._actor_location_map:
+ raise KeyError(
+ "Vehicle '{}' already registered. Cannot register twice!".format(actor.id))
+ else:
+ CarlaDataProvider._actor_location_map[actor] = None
+
+ if actor in CarlaDataProvider._actor_transform_map:
+ raise KeyError(
+ "Vehicle '{}' already registered. Cannot register twice!".format(actor.id))
+ else:
+ CarlaDataProvider._actor_transform_map[actor] = None
+
+ @staticmethod
+ def register_actors(actors):
+ """
+ Add new set of actors to dictionaries
+ """
+ for actor in actors:
+ CarlaDataProvider.register_actor(actor)
+
+ @staticmethod
+ def on_carla_tick():
+ """
+ Callback from CARLA
+ """
+ for actor in CarlaDataProvider._actor_velocity_map:
+ if actor is not None and actor.is_alive:
+ CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity(actor)
+
+ for actor in CarlaDataProvider._actor_location_map:
+ if actor is not None and actor.is_alive:
+ CarlaDataProvider._actor_location_map[actor] = actor.get_location()
+
+ for actor in CarlaDataProvider._actor_transform_map:
+ if actor is not None and actor.is_alive:
+ CarlaDataProvider._actor_transform_map[actor] = actor.get_transform()
+
+ world = CarlaDataProvider._world
+ if world is None:
+ print("WARNING: CarlaDataProvider couldn't find the world")
+
+ @staticmethod
+ def get_velocity(actor):
+ """
+ returns the absolute velocity for the given actor
+ """
+ for key in CarlaDataProvider._actor_velocity_map:
+ if key.id == actor.id:
+ return CarlaDataProvider._actor_velocity_map[key]
+
+ # We are intentionally not throwing here
+ # This may cause exception loops in py_trees
+ print('{}.get_velocity: {} not found!' .format(__name__, actor))
+ return 0.0
+
+ @staticmethod
+ def get_location(actor):
+ """
+ returns the location for the given actor
+ """
+ for key in CarlaDataProvider._actor_location_map:
+ if key.id == actor.id:
+ return CarlaDataProvider._actor_location_map[key]
+
+ # We are intentionally not throwing here
+ # This may cause exception loops in py_trees
+ print('{}.get_location: {} not found!' .format(__name__, actor))
+ return None
+
+ @staticmethod
+ def get_transform(actor):
+ """
+ returns the transform for the given actor
+ """
+ for key in CarlaDataProvider._actor_transform_map:
+ if key.id == actor.id:
+ return CarlaDataProvider._actor_transform_map[key]
+
+ # We are intentionally not throwing here
+ # This may cause exception loops in py_trees
+ print('{}.get_transform: {} not found!' .format(__name__, actor))
+ return None
+
+ @staticmethod
+ def set_client(client):
+ """
+ Set the CARLA client
+ """
+ CarlaDataProvider._client = client
+
+ @staticmethod
+ def get_client():
+ """
+ Get the CARLA client
+ """
+ return CarlaDataProvider._client
+
+ @staticmethod
+ def set_ego(vehicle):
+ """
+ Set the ego vehicle
+ """
+ CarlaDataProvider._ego_vehicle = vehicle
+
+ @staticmethod
+ def get_ego():
+ """
+ Get the CARLA ego
+ """
+ return CarlaDataProvider._ego_vehicle
+
+ @staticmethod
+ def set_world(world):
+ """
+ Set the world and world settings
+ """
+ CarlaDataProvider._world = world
+ CarlaDataProvider._sync_flag = world.get_settings().synchronous_mode
+ CarlaDataProvider._map = world.get_map()
+ CarlaDataProvider._blueprint_library = world.get_blueprint_library()
+ CarlaDataProvider.generate_spawn_points()
+ CarlaDataProvider.prepare_map()
+
+ @staticmethod
+ def set_weather(weather):
+ """
+ Set the weather of the world
+ """
+ CarlaDataProvider._world.set_weather(weather)
+
+ @staticmethod
+ def get_world():
+ """
+ Return world
+ """
+ return CarlaDataProvider._world
+
+ @staticmethod
+ def get_map(world=None):
+ """
+ Get the current map
+ """
+ if CarlaDataProvider._map is None:
+ if world is None:
+ if CarlaDataProvider._world is None:
+ raise ValueError("class member \'world'\' not initialized yet")
+ else:
+ CarlaDataProvider._map = CarlaDataProvider._world.get_map()
+ else:
+ CarlaDataProvider._map = world.get_map()
+
+ return CarlaDataProvider._map
+
+ @staticmethod
+ def is_sync_mode():
+ """
+ @return true if syncronuous mode is used
+ """
+ return CarlaDataProvider._sync_flag
+
+ @staticmethod
+ def find_weather_presets():
+ """
+ Get weather presets from CARLA
+ """
+ rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)')
+ name = lambda x: ' '.join(m.group(0) for m in rgx.finditer(x))
+ presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)]
+ return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets]
+
+ @staticmethod
+ def prepare_map():
+ """
+ This function set the current map and loads all traffic lights for this map to
+ _traffic_light_map
+ """
+ if CarlaDataProvider._map is None:
+ CarlaDataProvider._map = CarlaDataProvider._world.get_map()
+
+ # Parse all traffic lights
+ CarlaDataProvider._traffic_light_map.clear()
+ for traffic_light in CarlaDataProvider._world.get_actors().filter('*traffic_light*'):
+ if traffic_light not in CarlaDataProvider._traffic_light_map.keys():
+ CarlaDataProvider._traffic_light_map[traffic_light] = traffic_light.get_transform()
+ else:
+ raise KeyError(
+ "Traffic light '{}' already registered. Cannot register twice!".format(traffic_light.id))
+
+ @staticmethod
+ def annotate_trafficlight_in_group(traffic_light):
+ """
+ Get dictionary with traffic light group info for a given traffic light
+ """
+ dict_annotations = {'ref': [], 'opposite': [], 'left': [], 'right': []}
+
+ # Get the waypoints
+ ref_location = CarlaDataProvider.get_trafficlight_trigger_location(traffic_light)
+ ref_waypoint = CarlaDataProvider.get_map().get_waypoint(ref_location)
+ ref_yaw = ref_waypoint.transform.rotation.yaw
+
+ group_tl = traffic_light.get_group_traffic_lights()
+
+ for target_tl in group_tl:
+ if traffic_light.id == target_tl.id:
+ dict_annotations['ref'].append(target_tl)
+ else:
+ # Get the angle between yaws
+ target_location = CarlaDataProvider.get_trafficlight_trigger_location(target_tl)
+ target_waypoint = CarlaDataProvider.get_map().get_waypoint(target_location)
+ target_yaw = target_waypoint.transform.rotation.yaw
+
+ diff = (target_yaw - ref_yaw) % 360
+
+ if diff > 330:
+ continue
+ elif diff > 225:
+ dict_annotations['right'].append(target_tl)
+ elif diff > 135.0:
+ dict_annotations['opposite'].append(target_tl)
+ elif diff > 30:
+ dict_annotations['left'].append(target_tl)
+
+ return dict_annotations
+
+ @staticmethod
+ def get_trafficlight_trigger_location(traffic_light): # pylint: disable=invalid-name
+ """
+ Calculates the yaw of the waypoint that represents the trigger volume of the traffic light
+ """
+ def rotate_point(point, angle):
+ """
+ rotate a given point by a given angle
+ """
+ x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y
+ y_ = math.sin(math.radians(angle)) * point.x - math.cos(math.radians(angle)) * point.y
+
+ return carla.Vector3D(x_, y_, point.z)
+
+ base_transform = traffic_light.get_transform()
+ base_rot = base_transform.rotation.yaw
+ area_loc = base_transform.transform(traffic_light.trigger_volume.location)
+ area_ext = traffic_light.trigger_volume.extent
+
+ point = rotate_point(carla.Vector3D(0, 0, area_ext.z), base_rot)
+ point_location = area_loc + carla.Location(x=point.x, y=point.y)
+
+ return carla.Location(point_location.x, point_location.y, point_location.z)
+
+ @staticmethod
+ def update_light_states(ego_light, annotations, states, freeze=False, timeout=1000000000):
+ """
+ Update traffic light states
+ """
+ reset_params = []
+
+ for state in states:
+ relevant_lights = []
+ if state == 'ego':
+ relevant_lights = [ego_light]
+ else:
+ relevant_lights = annotations[state]
+ for light in relevant_lights:
+ prev_state = light.get_state()
+ prev_green_time = light.get_green_time()
+ prev_red_time = light.get_red_time()
+ prev_yellow_time = light.get_yellow_time()
+ reset_params.append({'light': light,
+ 'state': prev_state,
+ 'green_time': prev_green_time,
+ 'red_time': prev_red_time,
+ 'yellow_time': prev_yellow_time})
+
+ light.set_state(states[state])
+ if freeze:
+ light.set_green_time(timeout)
+ light.set_red_time(timeout)
+ light.set_yellow_time(timeout)
+
+ return reset_params
+
+ @staticmethod
+ def reset_lights(reset_params):
+ """
+ Reset traffic lights
+ """
+ for param in reset_params:
+ param['light'].set_state(param['state'])
+ param['light'].set_green_time(param['green_time'])
+ param['light'].set_red_time(param['red_time'])
+ param['light'].set_yellow_time(param['yellow_time'])
+
+ @staticmethod
+ def get_next_traffic_light(actor, use_cached_location=True):
+ """
+ returns the next relevant traffic light for the provided actor
+ """
+
+ if not use_cached_location:
+ location = actor.get_transform().location
+ else:
+ location = CarlaDataProvider.get_location(actor)
+
+ waypoint = CarlaDataProvider.get_map().get_waypoint(location)
+ # Create list of all waypoints until next intersection
+ list_of_waypoints = []
+ while waypoint and not waypoint.is_intersection:
+ list_of_waypoints.append(waypoint)
+ waypoint = waypoint.next(2.0)[0]
+
+ # If the list is empty, the actor is in an intersection
+ if not list_of_waypoints:
+ return None
+
+ relevant_traffic_light = None
+ distance_to_relevant_traffic_light = float("inf")
+
+ for traffic_light in CarlaDataProvider._traffic_light_map:
+ if hasattr(traffic_light, 'trigger_volume'):
+ tl_t = CarlaDataProvider._traffic_light_map[traffic_light]
+ transformed_tv = tl_t.transform(traffic_light.trigger_volume.location)
+ distance = carla.Location(transformed_tv).distance(list_of_waypoints[-1].transform.location)
+
+ if distance < distance_to_relevant_traffic_light:
+ relevant_traffic_light = traffic_light
+ distance_to_relevant_traffic_light = distance
+
+ return relevant_traffic_light
+
+ @staticmethod
+ def set_ego_vehicle_route(route):
+ """
+ Set the route of the ego vehicle
+
+ @todo extend ego_vehicle_route concept to support multi ego_vehicle scenarios
+ """
+ CarlaDataProvider._ego_vehicle_route = route
+
+ @staticmethod
+ def get_ego_vehicle_route():
+ """
+ returns the currently set route of the ego vehicle
+ Note: Can be None
+ """
+ return CarlaDataProvider._ego_vehicle_route
+
+ @staticmethod
+ def generate_spawn_points():
+ """
+ Generate spawn points for the current map
+ """
+ spawn_points = list(CarlaDataProvider.get_map(CarlaDataProvider._world).get_spawn_points())
+ CarlaDataProvider._rng.shuffle(spawn_points)
+ CarlaDataProvider._spawn_points = spawn_points
+ CarlaDataProvider._spawn_index = 0
+
+ @staticmethod
+ def create_blueprint(model, rolename='scenario', color=None, actor_category="car"):
+ """
+ Function to setup the blueprint of an actor given its model and other relevant parameters
+ """
+
+ _actor_blueprint_categories = {
+ 'car': 'vehicle.tesla.model3',
+ 'van': 'vehicle.volkswagen.t2',
+ 'truck': 'vehicle.carlamotors.carlacola',
+ 'trailer': '',
+ 'semitrailer': '',
+ 'bus': 'vehicle.volkswagen.t2',
+ 'motorbike': 'vehicle.kawasaki.ninja',
+ 'bicycle': 'vehicle.diamondback.century',
+ 'train': '',
+ 'tram': '',
+ 'pedestrian': 'walker.pedestrian.0001',
+ }
+
+ # Set the model
+ try:
+ blueprint = CarlaDataProvider._rng.choice(CarlaDataProvider._blueprint_library.filter(model))
+ except ValueError:
+ # The model is not part of the blueprint library. Let's take a default one for the given category
+ bp_filter = "vehicle.*"
+ new_model = _actor_blueprint_categories[actor_category]
+ if new_model != '':
+ bp_filter = new_model
+ print("WARNING: Actor model {} not available. Using instead {}".format(model, new_model))
+ blueprint = CarlaDataProvider._rng.choice(CarlaDataProvider._blueprint_library.filter(bp_filter))
+
+ # Set the color
+ if color:
+ if not blueprint.has_attribute('color'):
+ print(
+ "WARNING: Cannot set Color ({}) for actor {} due to missing blueprint attribute".format(
+ color, blueprint.id))
+ else:
+ default_color_rgba = blueprint.get_attribute('color').as_color()
+ default_color = '({}, {}, {})'.format(default_color_rgba.r, default_color_rgba.g, default_color_rgba.b)
+ try:
+ blueprint.set_attribute('color', color)
+ except ValueError:
+ # Color can't be set for this vehicle
+ print("WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format(
+ color, blueprint.id, default_color))
+ blueprint.set_attribute('color', default_color)
+ else:
+ if blueprint.has_attribute('color') and rolename != 'hero':
+ color = CarlaDataProvider._rng.choice(blueprint.get_attribute('color').recommended_values)
+ blueprint.set_attribute('color', color)
+
+ # Make pedestrians mortal
+ if blueprint.has_attribute('is_invincible'):
+ blueprint.set_attribute('is_invincible', 'false')
+
+ # Set the rolename
+ if blueprint.has_attribute('role_name'):
+ blueprint.set_attribute('role_name', rolename)
+
+ return blueprint
+
+ @staticmethod
+ def handle_actor_batch(batch):
+ """
+ Forward a CARLA command batch to spawn actors to CARLA, and gather the responses.
+ Returns list of actors on success, none otherwise
+ """
+
+ actors = []
+
+ sync_mode = CarlaDataProvider.is_sync_mode()
+
+ if CarlaDataProvider._client and batch is not None:
+ responses = CarlaDataProvider._client.apply_batch_sync(batch, sync_mode)
+ else:
+ return None
+
+ # wait for the actors to be spawned properly before we do anything
+ if sync_mode:
+ CarlaDataProvider._world.tick()
+ else:
+ CarlaDataProvider._world.wait_for_tick()
+
+ actor_ids = []
+ if responses:
+ for response in responses:
+ if not response.error:
+ actor_ids.append(response.actor_id)
+
+ carla_actors = CarlaDataProvider._world.get_actors(actor_ids)
+ for actor in carla_actors:
+ actors.append(actor)
+
+ return actors
+
+ @staticmethod
+ def request_new_actor(model, spawn_point, rolename='scenario', autopilot=False,
+ random_location=False, color=None, actor_category="car"):
+ """
+ This method tries to create a new actor, returning it if successful (None otherwise).
+ """
+ blueprint = CarlaDataProvider.create_blueprint(model, rolename, color, actor_category)
+
+ if random_location:
+ actor = None
+ while not actor:
+ spawn_point = CarlaDataProvider._rng.choice(CarlaDataProvider._spawn_points)
+ actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point)
+
+ else:
+ # slightly lift the actor to avoid collisions with ground when spawning the actor
+ # DO NOT USE spawn_point directly, as this will modify spawn_point permanently
+ _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation)
+ _spawn_point.location.x = spawn_point.location.x
+ _spawn_point.location.y = spawn_point.location.y
+ _spawn_point.location.z = spawn_point.location.z + 0.2
+ actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point)
+
+ if actor is None:
+ raise RuntimeError(
+ "Error: Unable to spawn vehicle {} at {}".format(blueprint.id, spawn_point))
+ else:
+ # Let's deactivate the autopilot of the actor if it belongs to vehicle
+ if actor in CarlaDataProvider._blueprint_library.filter('vehicle.*'):
+ actor.set_autopilot(autopilot)
+ else:
+ pass
+
+ # wait for the actor to be spawned properly before we do anything
+ if CarlaDataProvider.is_sync_mode():
+ CarlaDataProvider._world.tick()
+ else:
+ CarlaDataProvider._world.wait_for_tick()
+
+ if actor is None:
+ return None
+
+ CarlaDataProvider._carla_actor_pool[actor.id] = actor
+ CarlaDataProvider.register_actor(actor)
+ return actor
+
+ @staticmethod
+ def request_new_actors(actor_list):
+ """
+ This method tries to series of actor in batch. If this was successful,
+ the new actors are returned, None otherwise.
+
+ param:
+ - actor_list: list of ActorConfigurationData
+ """
+
+ SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name
+ PhysicsCommand = carla.command.SetSimulatePhysics # pylint: disable=invalid-name
+ FutureActor = carla.command.FutureActor # pylint: disable=invalid-name
+ ApplyTransform = carla.command.ApplyTransform # pylint: disable=invalid-name
+ SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name
+
+ batch = []
+ actors = []
+
+ CarlaDataProvider.generate_spawn_points()
+
+ for actor in actor_list:
+
+ # Get the blueprint
+ blueprint = CarlaDataProvider.create_blueprint(actor.model, actor.rolename, actor.color, actor.category)
+
+ # Get the spawn point
+ transform = actor.transform
+ if actor.random_location:
+ if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points):
+ print("No more spawn points to use")
+ break
+ else:
+ _spawn_point = CarlaDataProvider._spawn_points[CarlaDataProvider._spawn_index]
+ CarlaDataProvider._spawn_index += 1
+
+ else:
+ _spawn_point = carla.Transform()
+ _spawn_point.rotation = transform.rotation
+ _spawn_point.location.x = transform.location.x
+ _spawn_point.location.y = transform.location.y
+ _spawn_point.location.z = transform.location.z + 0.2
+
+ # Get the command
+ command = SpawnActor(blueprint, _spawn_point)
+ command.then(SetAutopilot(FutureActor, actor.autopilot,
+ CarlaDataProvider._traffic_manager_port))
+
+ if actor.category == 'misc':
+ command.then(PhysicsCommand(FutureActor, True))
+ elif actor.args is not None and 'physics' in actor.args and actor.args['physics'] == "off":
+ command.then(ApplyTransform(FutureActor, _spawn_point)).then(PhysicsCommand(FutureActor, False))
+
+ batch.append(command)
+
+ actors = CarlaDataProvider.handle_actor_batch(batch)
+
+ if not actors:
+ return None
+
+ for actor in actors:
+ if actor is None:
+ continue
+ CarlaDataProvider._carla_actor_pool[actor.id] = actor
+ CarlaDataProvider.register_actor(actor)
+ return actors
+
+ @staticmethod
+ def request_new_batch_actors(model, amount, spawn_points, autopilot=False,
+ random_location=False, rolename='scenario'):
+ """
+ Simplified version of "request_new_actors". This method also create several actors in batch.
+
+ Instead of needing a list of ActorConfigurationData, an "amount" parameter is used.
+ This makes actor spawning easier but reduces the amount of configurability.
+
+ Some parameters are the same for all actors (rolename, autopilot and random location)
+ while others are randomized (color)
+ """
+
+ SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name
+ SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name
+ FutureActor = carla.command.FutureActor # pylint: disable=invalid-name
+
+ CarlaDataProvider.generate_spawn_points()
+
+ batch = []
+
+ for i in range(amount):
+ # Get vehicle by model
+ blueprint = CarlaDataProvider.create_blueprint(model, rolename)
+
+ if random_location:
+ if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points):
+ print("No more spawn points to use. Spawned {} actors out of {}".format(i + 1, amount))
+ break
+ else:
+ spawn_point = CarlaDataProvider._spawn_points[CarlaDataProvider._spawn_index]
+ CarlaDataProvider._spawn_index += 1
+ else:
+ try:
+ spawn_point = spawn_points[i]
+ except IndexError:
+ print("The amount of spawn points is lower than the amount of vehicles spawned")
+ break
+
+ if spawn_point:
+ batch.append(SpawnActor(blueprint, spawn_point).then(
+ SetAutopilot(FutureActor, autopilot,
+ CarlaDataProvider._traffic_manager_port)))
+
+ actors = CarlaDataProvider.handle_actor_batch(batch)
+
+ if actors is None:
+ return None
+
+ for actor in actors:
+ if actor is None:
+ continue
+ CarlaDataProvider._carla_actor_pool[actor.id] = actor
+ CarlaDataProvider.register_actor(actor)
+ return actors
+
+ @staticmethod
+ def get_actors():
+ """
+ Return list of actors and their ids
+
+ Note: iteritems from six is used to allow compatibility with Python 2 and 3
+ """
+ return iteritems(CarlaDataProvider._carla_actor_pool)
+
+ @staticmethod
+ def actor_id_exists(actor_id):
+ """
+ Check if a certain id is still at the simulation
+ """
+ if actor_id in CarlaDataProvider._carla_actor_pool:
+ return True
+
+ return False
+
+ @staticmethod
+ def get_hero_actor():
+ """
+ Get the actor object of the hero actor if it exists, returns none otherwise.
+ """
+ for actor_id in CarlaDataProvider._carla_actor_pool:
+ if CarlaDataProvider._carla_actor_pool[actor_id].attributes['role_name'] == 'hero':
+ return CarlaDataProvider._carla_actor_pool[actor_id]
+ return None
+
+ @staticmethod
+ def get_actor_by_id(actor_id):
+ """
+ Get an actor from the pool by using its ID. If the actor
+ does not exist, None is returned.
+ """
+ if actor_id in CarlaDataProvider._carla_actor_pool:
+ return CarlaDataProvider._carla_actor_pool[actor_id]
+
+ print("Non-existing actor id {}".format(actor_id))
+ return None
+
+ @staticmethod
+ def remove_actor_by_id(actor_id):
+ """
+ Remove an actor from the pool using its ID
+ """
+ if actor_id in CarlaDataProvider._carla_actor_pool:
+ CarlaDataProvider._carla_actor_pool[actor_id].destroy()
+ CarlaDataProvider._carla_actor_pool[actor_id] = None
+ CarlaDataProvider._carla_actor_pool.pop(actor_id)
+ else:
+ print("Trying to remove a non-existing actor id {}".format(actor_id))
+
+ @staticmethod
+ def remove_actors_in_surrounding(location, distance):
+ """
+ Remove all actors from the pool that are closer than distance to the
+ provided location
+ """
+ for actor_id in CarlaDataProvider._carla_actor_pool.copy():
+ if CarlaDataProvider._carla_actor_pool[actor_id].get_location().distance(location) < distance:
+ CarlaDataProvider._carla_actor_pool[actor_id].destroy()
+ CarlaDataProvider._carla_actor_pool.pop(actor_id)
+
+ # Remove all keys with None values
+ CarlaDataProvider._carla_actor_pool = dict({k: v for k, v in CarlaDataProvider._carla_actor_pool.items() if v})
+
+ @staticmethod
+ def get_traffic_manager_port():
+ """
+ Get the port of the traffic manager.
+ """
+ return CarlaDataProvider._traffic_manager_port
+
+ @staticmethod
+ def set_traffic_manager_port(tm_port):
+ """
+ Set the port to use for the traffic manager.
+ """
+ CarlaDataProvider._traffic_manager_port = tm_port
+
+ @staticmethod
+ def cleanup():
+ """
+ Cleanup and remove all entries from all dictionaries
+ """
+ DestroyActor = carla.command.DestroyActor # pylint: disable=invalid-name
+ batch = []
+
+ for actor_id in CarlaDataProvider._carla_actor_pool.copy():
+ actor = CarlaDataProvider._carla_actor_pool[actor_id]
+ if actor.is_alive:
+ batch.append(DestroyActor(actor))
+
+ if CarlaDataProvider._client:
+ try:
+ CarlaDataProvider._client.apply_batch_sync(batch)
+ except RuntimeError as e:
+ if "time-out" in str(e):
+ pass
+ else:
+ raise e
+
+ CarlaDataProvider._actor_velocity_map.clear()
+ CarlaDataProvider._actor_location_map.clear()
+ CarlaDataProvider._actor_transform_map.clear()
+ CarlaDataProvider._traffic_light_map.clear()
+ CarlaDataProvider._map = None
+ CarlaDataProvider._world = None
+ CarlaDataProvider._sync_flag = False
+ CarlaDataProvider._ego_vehicle_route = None
+ CarlaDataProvider._carla_actor_pool = dict()
+ CarlaDataProvider._client = None
+ CarlaDataProvider._spawn_points = None
+ CarlaDataProvider._spawn_index = 0
+ CarlaDataProvider._ego_vehicle = None
+ CarlaDataProvider._rng = random.RandomState(2000)
diff --git a/scenario_runner/srunner/scenariomanager/result_writer.py b/scenario_runner/srunner/scenariomanager/result_writer.py
new file mode 100644
index 0000000..28ae2a8
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/result_writer.py
@@ -0,0 +1,217 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module contains the result gatherer and write for CARLA scenarios.
+It shall be used from the ScenarioManager only.
+"""
+
+from __future__ import print_function
+
+import time
+from tabulate import tabulate
+
+
+class ResultOutputProvider(object):
+
+ """
+ This module contains the _result gatherer and write for CARLA scenarios.
+ It shall be used from the ScenarioManager only.
+ """
+
+ def __init__(self, data, result, stdout=True, filename=None, junit=None):
+ """
+ Setup all parameters
+ - _data contains all scenario-related information
+ - _result is overall pass/fail info
+ - _stdout (True/False) is used to (de)activate terminal output
+ - _filename is used to (de)activate file output in tabular form
+ - _junit is used to (de)activate file output in _junit form
+ """
+ self._data = data
+ self._result = result
+ self._stdout = stdout
+ self._filename = filename
+ self._junit = junit
+
+ self._start_time = time.strftime('%Y-%m-%d %H:%M:%S',
+ time.localtime(self._data.start_system_time))
+ self._end_time = time.strftime('%Y-%m-%d %H:%M:%S',
+ time.localtime(self._data.end_system_time))
+
+ def write(self):
+ """
+ Public write function
+ """
+ if self._junit is not None:
+ self._write_to_junit()
+
+ output = self.create_output_text()
+ if self._filename is not None:
+ with open(self._filename, 'w') as fd:
+ fd.write(output)
+ if self._stdout:
+ print(output)
+
+ def create_output_text(self):
+ """
+ Creates the output message
+ """
+ output = "\n"
+ output += " ======= Results of Scenario: {} ---- {} =======\n".format(
+ self._data.scenario_tree.name, self._result)
+ end_line_length = len(output) - 3
+ output += "\n"
+
+ # Lis of all the actors
+ output += " > Ego vehicles:\n"
+ for ego_vehicle in self._data.ego_vehicles:
+ output += "{}; ".format(ego_vehicle)
+ output += "\n\n"
+
+ output += " > Other actors:\n"
+ for actor in self._data.other_actors:
+ output += "{}; ".format(actor)
+ output += "\n\n"
+
+ # Simulation part
+ output += " > Simulation Information\n"
+
+ system_time = round(self._data.scenario_duration_system, 2)
+ game_time = round(self._data.scenario_duration_game, 2)
+ ratio = round(self._data.scenario_duration_game / self._data.scenario_duration_system, 3)
+
+ list_statistics = [["Start Time", "{}".format(self._start_time)]]
+ list_statistics.extend([["End Time", "{}".format(self._end_time)]])
+ list_statistics.extend([["Duration (System Time)", "{}s".format(system_time)]])
+ list_statistics.extend([["Duration (Game Time)", "{}s".format(game_time)]])
+ list_statistics.extend([["Ratio (System Time / Game Time)", "{}s".format(ratio)]])
+
+ output += tabulate(list_statistics, tablefmt='fancy_grid')
+ output += "\n\n"
+
+ # Criteria part
+ output += " > Criteria Information\n"
+ header = ['Actor', 'Criterion', 'Result', 'Actual Value', 'Expected Value']
+ list_statistics = [header]
+
+ for criterion in self._data.scenario.get_criteria():
+ name_string = criterion.name
+ if criterion.optional:
+ name_string += " (Opt.)"
+ else:
+ name_string += " (Req.)"
+
+ actor = "{} (id={})".format(criterion.actor.type_id[8:], criterion.actor.id)
+ criteria = name_string
+ result = "FAILURE" if criterion.test_status == "RUNNING" else criterion.test_status
+ actual_value = criterion.actual_value
+ expected_value = criterion.expected_value_success
+
+ list_statistics.extend([[actor, criteria, result, actual_value, expected_value]])
+
+ # Timeout
+ actor = ""
+ criteria = "Timeout (Req.)"
+ result = "SUCCESS" if self._data.scenario_duration_game < self._data.scenario.timeout else "FAILURE"
+ actual_value = round(self._data.scenario_duration_game, 2)
+ expected_value = round(self._data.scenario.timeout, 2)
+
+ list_statistics.extend([[actor, criteria, result, actual_value, expected_value]])
+
+ # Global and final output message
+ list_statistics.extend([['', 'GLOBAL RESULT', self._result, '', '']])
+
+ output += tabulate(list_statistics, tablefmt='fancy_grid')
+ output += "\n"
+ output += " " + "=" * end_line_length + "\n"
+
+ return output
+
+ def _write_to_junit(self):
+ """
+ Writing to Junit XML
+ """
+ test_count = 0
+ failure_count = 0
+ for criterion in self._data.scenario.get_criteria():
+ test_count += 1
+ if criterion.test_status != "SUCCESS":
+ failure_count += 1
+
+ # handle timeout
+ test_count += 1
+ if self._data.scenario_duration_game >= self._data.scenario.timeout:
+ failure_count += 1
+
+ junit_file = open(self._junit, "w")
+
+ junit_file.write("\n")
+
+ test_suites_string = ("\n" %
+ (test_count,
+ failure_count,
+ self._start_time,
+ self._data.scenario_duration_system))
+ junit_file.write(test_suites_string)
+
+ test_suite_string = (" \n" %
+ (self._data.scenario_tree.name,
+ test_count,
+ failure_count,
+ self._data.scenario_duration_system))
+ junit_file.write(test_suite_string)
+
+ for criterion in self._data.scenario.get_criteria():
+ testcase_name = criterion.name + "_" + \
+ criterion.actor.type_id[8:] + "_" + str(criterion.actor.id)
+ result_string = (" \n".format(
+ testcase_name, self._data.scenario_tree.name))
+ if criterion.test_status != "SUCCESS":
+ result_string += " \n".format(
+ criterion.name, criterion.actual_value)
+ else:
+ result_string += " Exact Value: {} = {}\n".format(
+ criterion.name, criterion.actual_value)
+ result_string += " \n"
+ junit_file.write(result_string)
+
+ # Handle timeout separately
+ result_string = (" \n".format(
+ self._data.scenario_duration_system,
+ self._data.scenario_tree.name))
+ if self._data.scenario_duration_game >= self._data.scenario.timeout:
+ result_string += " \n".format(
+ "Duration", self._data.scenario_duration_game)
+ else:
+ result_string += " Exact Value: {} = {}\n".format(
+ "Duration", self._data.scenario_duration_game)
+ result_string += " \n"
+ junit_file.write(result_string)
+
+ junit_file.write(" \n")
+ junit_file.write("\n")
+ junit_file.close()
diff --git a/scenario_runner/srunner/scenariomanager/scenario_manager.py b/scenario_runner/srunner/scenariomanager/scenario_manager.py
new file mode 100644
index 0000000..13a1474
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/scenario_manager.py
@@ -0,0 +1,231 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides the ScenarioManager implementation.
+It must not be modified and is for reference only!
+"""
+
+from __future__ import print_function
+import sys
+import time
+
+import py_trees
+
+from srunner.autoagents.agent_wrapper import AgentWrapper
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.result_writer import ResultOutputProvider
+from srunner.scenariomanager.timer import GameTime
+from srunner.scenariomanager.watchdog import Watchdog
+
+
+class ScenarioManager(object):
+
+ """
+ Basic scenario manager class. This class holds all functionality
+ required to start, and analyze a scenario.
+
+ The user must not modify this class.
+
+ To use the ScenarioManager:
+ 1. Create an object via manager = ScenarioManager()
+ 2. Load a scenario via manager.load_scenario()
+ 3. Trigger the execution of the scenario manager.run_scenario()
+ This function is designed to explicitly control start and end of
+ the scenario execution
+ 4. Trigger a result evaluation with manager.analyze_scenario()
+ 5. If needed, cleanup with manager.stop_scenario()
+ """
+
+ def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0):
+ """
+ Setups up the parameters, which will be filled at load_scenario()
+
+ """
+ self.scenario = None
+ self.scenario_tree = None
+ self.scenario_class = None
+ self.ego_vehicles = None
+ self.other_actors = None
+
+ self._debug_mode = debug_mode
+ self._agent = None
+ self._sync_mode = sync_mode
+ self._running = False
+ self._timestamp_last_run = 0.0
+ self._timeout = timeout
+ self._watchdog = Watchdog(float(self._timeout))
+
+ self.scenario_duration_system = 0.0
+ self.scenario_duration_game = 0.0
+ self.start_system_time = None
+ self.end_system_time = None
+
+ def _reset(self):
+ """
+ Reset all parameters
+ """
+ self._running = False
+ self._timestamp_last_run = 0.0
+ self.scenario_duration_system = 0.0
+ self.scenario_duration_game = 0.0
+ self.start_system_time = None
+ self.end_system_time = None
+ GameTime.restart()
+
+ def cleanup(self):
+ """
+ This function triggers a proper termination of a scenario
+ """
+
+ if self.scenario is not None:
+ self.scenario.terminate()
+
+ if self._agent is not None:
+ self._agent.cleanup()
+ self._agent = None
+
+ CarlaDataProvider.cleanup()
+
+ def load_scenario(self, scenario, agent=None):
+ """
+ Load a new scenario
+ """
+ self._reset()
+ self._agent = AgentWrapper(agent) if agent else None
+ if self._agent is not None:
+ self._sync_mode = True
+ self.scenario_class = scenario
+ self.scenario = scenario.scenario
+ self.scenario_tree = self.scenario.scenario_tree
+ self.ego_vehicles = scenario.ego_vehicles
+ self.other_actors = scenario.other_actors
+
+ # To print the scenario tree uncomment the next line
+ # py_trees.display.render_dot_tree(self.scenario_tree)
+
+ if self._agent is not None:
+ self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
+
+ def run_scenario(self):
+ """
+ Trigger the start of the scenario and wait for it to finish/fail
+ """
+ print("ScenarioManager: Running scenario {}".format(self.scenario_tree.name))
+ self.start_system_time = time.time()
+ start_game_time = GameTime.get_time()
+
+ self._watchdog.start()
+ self._running = True
+
+ while self._running:
+ timestamp = None
+ world = CarlaDataProvider.get_world()
+ if world:
+ snapshot = world.get_snapshot()
+ if snapshot:
+ timestamp = snapshot.timestamp
+ if timestamp:
+ self._tick_scenario(timestamp)
+
+ self._watchdog.stop()
+
+ self.cleanup()
+
+ self.end_system_time = time.time()
+ end_game_time = GameTime.get_time()
+
+ self.scenario_duration_system = self.end_system_time - \
+ self.start_system_time
+ self.scenario_duration_game = end_game_time - start_game_time
+
+ if self.scenario_tree.status == py_trees.common.Status.FAILURE:
+ print("ScenarioManager: Terminated due to failure")
+
+ def _tick_scenario(self, timestamp):
+ """
+ Run next tick of scenario and the agent.
+ If running synchornously, it also handles the ticking of the world.
+ """
+
+ if self._timestamp_last_run < timestamp.elapsed_seconds and self._running:
+ self._timestamp_last_run = timestamp.elapsed_seconds
+
+ self._watchdog.update()
+
+ if self._debug_mode:
+ print("\n--------- Tick ---------\n")
+
+ # Update game time and actor information
+ GameTime.on_carla_tick(timestamp)
+ CarlaDataProvider.on_carla_tick()
+
+ if self._agent is not None:
+ ego_action = self._agent()
+
+ # Tick scenario
+ self.scenario_tree.tick_once()
+
+ if self._debug_mode:
+ print("\n")
+ py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True)
+ sys.stdout.flush()
+
+ if self.scenario_tree.status != py_trees.common.Status.RUNNING:
+ self._running = False
+
+ if self._agent is not None:
+ self.ego_vehicles[0].apply_control(ego_action)
+
+ if self._sync_mode and self._running and self._watchdog.get_status():
+ CarlaDataProvider.get_world().tick()
+
+ def get_running_status(self):
+ """
+ returns:
+ bool: False if watchdog exception occured, True otherwise
+ """
+ return self._watchdog.get_status()
+
+ def stop_scenario(self):
+ """
+ This function is used by the overall signal handler to terminate the scenario execution
+ """
+ self._running = False
+
+ def analyze_scenario(self, stdout, filename, junit):
+ """
+ This function is intended to be called from outside and provide
+ the final statistics about the scenario (human-readable, in form of a junit
+ report, etc.)
+ """
+
+ failure = False
+ timeout = False
+ result = "SUCCESS"
+
+ if self.scenario.test_criteria is None:
+ print("Nothing to analyze, this scenario has no criteria")
+ return True
+
+ for criterion in self.scenario.get_criteria():
+ if (not criterion.optional and
+ criterion.test_status != "SUCCESS" and
+ criterion.test_status != "ACCEPTABLE"):
+ failure = True
+ result = "FAILURE"
+ elif criterion.test_status == "ACCEPTABLE":
+ result = "ACCEPTABLE"
+
+ if self.scenario.timeout_node.timeout and not failure:
+ timeout = True
+ result = "TIMEOUT"
+
+ output = ResultOutputProvider(self, result, stdout, filename, junit)
+ output.write()
+
+ return failure or timeout
diff --git a/scenario_runner/srunner/scenariomanager/scenarioatomics/__init__.py b/scenario_runner/srunner/scenariomanager/scenarioatomics/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py
new file mode 100644
index 0000000..309527f
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py
@@ -0,0 +1,2460 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides all atomic scenario behaviors required to realize
+complex, realistic scenarios such as "follow a leading vehicle", "lane change",
+etc.
+
+The atomic behaviors are implemented with py_trees.
+"""
+
+from __future__ import print_function
+
+import copy
+import math
+import operator
+import os
+import random
+import time
+import subprocess
+
+import numpy as np
+import py_trees
+from py_trees.blackboard import Blackboard
+
+import carla
+from agents.navigation.basic_agent import BasicAgent, LocalPlanner
+from agents.navigation.local_planner import RoadOption
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.actorcontrols.actor_control import ActorControl
+from srunner.scenariomanager.timer import GameTime
+from srunner.tools.scenario_helper import detect_lane_obstacle
+from srunner.tools.scenario_helper import generate_target_waypoint_list_multilane
+
+
+import srunner.tools
+
+EPSILON = 0.001
+
+
+def calculate_distance(location, other_location, global_planner=None):
+ """
+ Method to calculate the distance between to locations
+
+ Note: It uses the direct distance between the current location and the
+ target location to estimate the time to arrival.
+ To be accurate, it would have to use the distance along the
+ (shortest) route between the two locations.
+ """
+ if global_planner:
+ distance = 0
+
+ # Get the route
+ route = global_planner.trace_route(location, other_location)
+
+ # Get the distance of the route
+ for i in range(1, len(route)):
+ curr_loc = route[i][0].transform.location
+ prev_loc = route[i - 1][0].transform.location
+
+ distance += curr_loc.distance(prev_loc)
+
+ return distance
+
+ return location.distance(other_location)
+
+
+def get_actor_control(actor):
+ """
+ Method to return the type of control to the actor.
+ """
+ control = actor.get_control()
+ actor_type = actor.type_id.split('.')[0]
+ if not isinstance(actor, carla.Walker):
+ control.steering = 0
+ else:
+ control.speed = 0
+
+ return control, actor_type
+
+
+class AtomicBehavior(py_trees.behaviour.Behaviour):
+
+ """
+ Base class for all atomic behaviors used to setup a scenario
+
+ *All behaviors should use this class as parent*
+
+ Important parameters:
+ - name: Name of the atomic behavior
+ """
+
+ def __init__(self, name, actor=None):
+ """
+ Default init. Has to be called via super from derived class
+ """
+ super(AtomicBehavior, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self.name = name
+ self._actor = actor
+
+ def setup(self, unused_timeout=15):
+ """
+ Default setup
+ """
+ self.logger.debug("%s.setup()" % (self.__class__.__name__))
+ return True
+
+ def initialise(self):
+ """
+ Initialise setup terminates WaypointFollowers
+ Check whether WF for this actor is running and terminate all active WFs
+ """
+ if self._actor is not None:
+ try:
+ check_attr = operator.attrgetter("running_WF_actor_{}".format(self._actor.id))
+ terminate_wf = copy.copy(check_attr(py_trees.blackboard.Blackboard()))
+ py_trees.blackboard.Blackboard().set(
+ "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True)
+ except AttributeError:
+ # It is ok to continue, if the Blackboard variable does not exist
+ pass
+ self.logger.debug("%s.initialise()" % (self.__class__.__name__))
+
+ def terminate(self, new_status):
+ """
+ Default terminate. Can be extended in derived class
+ """
+ self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+
+class RunScript(AtomicBehavior):
+
+ """
+ This is an atomic behavior to start execution of an additional script.
+
+ Args:
+ script (str): String containing the interpreter, scriptpath and arguments
+ Example: "python /path/to/script.py --arg1"
+ base_path (str): String containing the base path of for the script
+
+ Attributes:
+ _script (str): String containing the interpreter, scriptpath and arguments
+ Example: "python /path/to/script.py --arg1"
+ _base_path (str): String containing the base path of for the script
+ Example: "/path/to/"
+
+ Note:
+ This is intended for the use with OpenSCENARIO. Be aware of security side effects.
+ """
+
+ def __init__(self, script, base_path=None, name="RunScript"):
+ """
+ Setup parameters
+ """
+ super(RunScript, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._script = script
+ self._base_path = base_path
+
+ def update(self):
+ """
+ Start script
+ """
+ path = None
+ script_components = self._script.split(' ')
+ if len(script_components) > 1:
+ path = script_components[1]
+
+ if not os.path.isfile(path):
+ path = os.path.join(self._base_path, path)
+ if not os.path.isfile(path):
+ new_status = py_trees.common.Status.FAILURE
+ print("Script file does not exists {}".format(path))
+ else:
+ subprocess.Popen(self._script, shell=True, cwd=self._base_path)
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+
+class ChangeWeather(AtomicBehavior):
+
+ """
+ Atomic to write a new weather configuration into the blackboard.
+ Used in combination with WeatherBehavior() to have a continuous weather simulation.
+
+ The behavior immediately terminates with SUCCESS after updating the blackboard.
+
+ Args:
+ weather (srunner.scenariomanager.weather_sim.Weather): New weather settings.
+ name (string): Name of the behavior.
+ Defaults to 'UpdateWeather'.
+
+ Attributes:
+ _weather (srunner.scenariomanager.weather_sim.Weather): Weather settings.
+ """
+
+ def __init__(self, weather, name="ChangeWeather"):
+ """
+ Setup parameters
+ """
+ super(ChangeWeather, self).__init__(name)
+ self._weather = weather
+
+ def update(self):
+ """
+ Write weather into blackboard and exit with success
+
+ returns:
+ py_trees.common.Status.SUCCESS
+ """
+ py_trees.blackboard.Blackboard().set("CarlaWeather", self._weather, overwrite=True)
+ return py_trees.common.Status.SUCCESS
+
+
+class ChangeRoadFriction(AtomicBehavior):
+
+ """
+ Atomic to update the road friction in CARLA.
+
+ The behavior immediately terminates with SUCCESS after updating the friction.
+
+ Args:
+ friction (float): New friction coefficient.
+ name (string): Name of the behavior.
+ Defaults to 'UpdateRoadFriction'.
+
+ Attributes:
+ _friction (float): Friction coefficient.
+ """
+
+ def __init__(self, friction, name="ChangeRoadFriction"):
+ """
+ Setup parameters
+ """
+ super(ChangeRoadFriction, self).__init__(name)
+ self._friction = friction
+
+ def update(self):
+ """
+ Update road friction. Spawns new friction blueprint and removes the old one, if existing.
+
+ returns:
+ py_trees.common.Status.SUCCESS
+ """
+
+ for actor in CarlaDataProvider.get_world().get_actors().filter('static.trigger.friction'):
+ actor.destroy()
+
+ friction_bp = CarlaDataProvider.get_world().get_blueprint_library().find('static.trigger.friction')
+ extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
+ friction_bp.set_attribute('friction', str(self._friction))
+ friction_bp.set_attribute('extent_x', str(extent.x))
+ friction_bp.set_attribute('extent_y', str(extent.y))
+ friction_bp.set_attribute('extent_z', str(extent.z))
+
+ # Spawn Trigger Friction
+ transform = carla.Transform()
+ transform.location = carla.Location(-10000.0, -10000.0, 0.0)
+ CarlaDataProvider.get_world().spawn_actor(friction_bp, transform)
+
+ return py_trees.common.Status.SUCCESS
+
+
+class ChangeActorControl(AtomicBehavior):
+
+ """
+ Atomic to change the longitudinal/lateral control logic for an actor.
+ The (actor, controller) pair is stored inside the Blackboard.
+
+ The behavior immediately terminates with SUCCESS after the controller.
+
+ Args:
+ actor (carla.Actor): Actor that should be controlled by the controller.
+ control_py_module (string): Name of the python module containing the implementation
+ of the controller.
+ args (dictionary): Additional arguments for the controller.
+ name (string): Name of the behavior.
+ Defaults to 'ChangeActorControl'.
+
+ Attributes:
+ _actor_control (ActorControl): Instance of the actor control.
+ """
+
+ def __init__(self, actor, control_py_module, args, name="ChangeActorControl"):
+ """
+ Setup actor controller.
+ """
+ super(ChangeActorControl, self).__init__(name, actor)
+
+ self._actor_control = ActorControl(actor, control_py_module=control_py_module, args=args)
+
+ def update(self):
+ """
+ Write (actor, controler) pair to Blackboard, or update the controller
+ if actor already exists as a key.
+
+ returns:
+ py_trees.common.Status.SUCCESS
+ """
+
+ actor_dict = {}
+
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if actor_dict:
+ if self._actor.id in actor_dict:
+ actor_dict[self._actor.id].reset()
+
+ actor_dict[self._actor.id] = self._actor_control
+ py_trees.blackboard.Blackboard().set("ActorsWithController", actor_dict, overwrite=True)
+
+ return py_trees.common.Status.SUCCESS
+
+
+class UpdateAllActorControls(AtomicBehavior):
+
+ """
+ Atomic to update (run one control loop step) all actor controls.
+
+ The behavior is always in RUNNING state.
+
+ Args:
+ name (string): Name of the behavior.
+ Defaults to 'UpdateAllActorControls'.
+ """
+
+ def __init__(self, name="UpdateAllActorControls"):
+ """
+ Constructor
+ """
+ super(UpdateAllActorControls, self).__init__(name)
+
+ def update(self):
+ """
+ Execute one control loop step for all actor controls.
+
+ returns:
+ py_trees.common.Status.RUNNING
+ """
+
+ actor_dict = {}
+
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ for actor_id in actor_dict:
+ actor_dict[actor_id].run_step()
+
+ return py_trees.common.Status.RUNNING
+
+
+class ChangeActorTargetSpeed(AtomicBehavior):
+
+ """
+ Atomic to change the target speed for an actor controller.
+
+ The behavior is in RUNNING state until the distance/duration
+ conditions are satisfied, or if a second ChangeActorTargetSpeed atomic
+ for the same actor is triggered.
+
+ Args:
+ actor (carla.Actor): Controlled actor.
+ target_speed (float): New target speed [m/s].
+ init_speed (boolean): Flag to indicate if the speed is the initial actor speed.
+ Defaults to False.
+ duration (float): Duration of the maneuver [s].
+ Defaults to None.
+ distance (float): Distance of the maneuver [m].
+ Defaults to None.
+ relative_actor (carla.Actor): If the target speed setting should be relative to another actor.
+ Defaults to None.
+ value (float): Offset, if the target speed setting should be relative to another actor.
+ Defaults to None.
+ value_type (string): Either 'Delta' or 'Factor' influencing how the offset to the reference actors
+ velocity is applied. Defaults to None.
+ continuous (boolean): If True, the atomic remains in RUNNING, independent of duration or distance.
+ Defaults to False.
+ name (string): Name of the behavior.
+ Defaults to 'ChangeActorTargetSpeed'.
+
+ Attributes:
+ _target_speed (float): New target speed [m/s].
+ _init_speed (boolean): Flag to indicate if the speed is the initial actor speed.
+ Defaults to False.
+ _start_time (float): Start time of the atomic [s].
+ Defaults to None.
+ _start_location (carla.Location): Start location of the atomic.
+ Defaults to None.
+ _duration (float): Duration of the maneuver [s].
+ Defaults to None.
+ _distance (float): Distance of the maneuver [m].
+ Defaults to None.
+ _relative_actor (carla.Actor): If the target speed setting should be relative to another actor.
+ Defaults to None.
+ _value (float): Offset, if the target speed setting should be relative to another actor.
+ Defaults to None.
+ _value_type (string): Either 'Delta' or 'Factor' influencing how the offset to the reference actors
+ velocity is applied. Defaults to None.
+ _continuous (boolean): If True, the atomic remains in RUNNING, independent of duration or distance.
+ Defaults to False.
+ """
+
+ def __init__(self, actor, target_speed, init_speed=False,
+ duration=None, distance=None, relative_actor=None,
+ value=None, value_type=None, continuous=False, name="ChangeActorTargetSpeed"):
+ """
+ Setup parameters
+ """
+ super(ChangeActorTargetSpeed, self).__init__(name, actor)
+
+ self._target_speed = target_speed
+ self._init_speed = init_speed
+
+ self._start_time = None
+ self._start_location = None
+
+ self._relative_actor = relative_actor
+ self._value = value
+ self._value_type = value_type
+ self._continuous = continuous
+ self._duration = duration
+ self._distance = distance
+
+ def initialise(self):
+ """
+ Set initial parameters such as _start_time and _start_location,
+ and get (actor, controller) pair from Blackboard.
+
+ May throw if actor is not available as key for the ActorsWithController
+ dictionary from Blackboard.
+ """
+ actor_dict = {}
+
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ raise RuntimeError("Actor not found in ActorsWithController BlackBoard")
+
+ self._start_time = GameTime.get_time()
+ self._start_location = CarlaDataProvider.get_location(self._actor)
+
+ if self._relative_actor:
+ relative_velocity = CarlaDataProvider.get_velocity(self._relative_actor)
+
+ # get target velocity
+ if self._value_type == 'delta':
+ self._target_speed = relative_velocity + self._value
+ elif self._value_type == 'factor':
+ self._target_speed = relative_velocity * self._value
+ else:
+ print('self._value_type must be delta or factor')
+
+ actor_dict[self._actor.id].update_target_speed(self._target_speed, start_time=self._start_time)
+
+ if self._init_speed:
+ actor_dict[self._actor.id].set_init_speed()
+
+ super(ChangeActorTargetSpeed, self).initialise()
+
+ def update(self):
+ """
+ Check the actor's current state and update target speed, if it is relative to another actor.
+
+ returns:
+ py_trees.common.Status.SUCCESS, if the duration or distance driven exceeded limits
+ if another ChangeActorTargetSpeed atomic for the same actor was triggered.
+ py_trees.common.Status.FAILURE, if the actor is not found in ActorsWithController Blackboard dictionary.
+ py_trees.common.Status.FAILURE, else.
+ """
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ return py_trees.common.Status.FAILURE
+
+ if actor_dict[self._actor.id].get_last_longitudinal_command() != self._start_time:
+ return py_trees.common.Status.SUCCESS
+
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._relative_actor:
+ relative_velocity = CarlaDataProvider.get_velocity(self._relative_actor)
+
+ # get target velocity
+ if self._value_type == 'delta':
+ actor_dict[self._actor.id].update_target_speed(relative_velocity + self._value)
+ elif self._value_type == 'factor':
+ actor_dict[self._actor.id].update_target_speed(relative_velocity * self._value)
+ else:
+ print('self._value_type must be delta or factor')
+
+ # check duration and driven_distance
+ if not self._continuous:
+ if (self._duration is not None) and (GameTime.get_time() - self._start_time > self._duration):
+ new_status = py_trees.common.Status.SUCCESS
+
+ driven_distance = CarlaDataProvider.get_location(self._actor).distance(self._start_location)
+ if (self._distance is not None) and (driven_distance > self._distance):
+ new_status = py_trees.common.Status.SUCCESS
+
+ if self._distance is None and self._duration is None:
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class ChangeActorWaypoints(AtomicBehavior):
+
+ """
+ Atomic to change the waypoints for an actor controller.
+
+ The behavior is in RUNNING state until the last waypoint is reached, or if a
+ second waypoint related atomic for the same actor is triggered. These are:
+ - ChangeActorWaypoints
+ - ChangeActorWaypointsToReachPosition
+ - ChangeActorLateralMotion
+
+ Args:
+ actor (carla.Actor): Controlled actor.
+ waypoints (List of carla.Transform): List of waypoints (CARLA transforms).
+ name (string): Name of the behavior.
+ Defaults to 'ChangeActorWaypoints'.
+
+ Attributes:
+ _waypoints (List of carla.Transform): List of waypoints (CARLA transforms).
+ _start_time (float): Start time of the atomic [s].
+ Defaults to None.
+ """
+
+ def __init__(self, actor, waypoints, name="ChangeActorWaypoints"):
+ """
+ Setup parameters
+ """
+ super(ChangeActorWaypoints, self).__init__(name, actor)
+
+ self._waypoints = waypoints
+ self._start_time = None
+
+ def initialise(self):
+ """
+ Set _start_time and get (actor, controller) pair from Blackboard.
+
+ Set waypoint list for actor controller.
+
+ May throw if actor is not available as key for the ActorsWithController
+ dictionary from Blackboard.
+ """
+ actor_dict = {}
+
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ raise RuntimeError("Actor not found in ActorsWithController BlackBoard")
+
+ self._start_time = GameTime.get_time()
+
+ actor_dict[self._actor.id].update_waypoints(self._waypoints, start_time=self._start_time)
+
+ super(ChangeActorWaypoints, self).initialise()
+
+ def update(self):
+ """
+ Check the actor's state along the waypoint route.
+
+ returns:
+ py_trees.common.Status.SUCCESS, if the final waypoint was reached, or
+ if another ChangeActorWaypoints atomic for the same actor was triggered.
+ py_trees.common.Status.FAILURE, if the actor is not found in ActorsWithController Blackboard dictionary.
+ py_trees.common.Status.FAILURE, else.
+ """
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ return py_trees.common.Status.FAILURE
+
+ if actor_dict[self._actor.id].get_last_waypoint_command() != self._start_time:
+ return py_trees.common.Status.SUCCESS
+
+ new_status = py_trees.common.Status.RUNNING
+
+ if actor_dict[self._actor.id].check_reached_waypoint_goal():
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class ChangeActorWaypointsToReachPosition(ChangeActorWaypoints):
+
+ """
+ Atomic to change the waypoints for an actor controller in order to reach
+ a given position.
+
+ The behavior is in RUNNING state until the last waypoint is reached, or if a
+ second waypoint related atomic for the same actor is triggered. These are:
+ - ChangeActorWaypoints
+ - ChangeActorWaypointsToReachPosition
+ - ChangeActorLateralMotion
+
+ Args:
+ actor (carla.Actor): Controlled actor.
+ position (carla.Transform): CARLA transform to be reached by the actor.
+ name (string): Name of the behavior.
+ Defaults to 'ChangeActorWaypointsToReachPosition'.
+
+ Attributes:
+ _waypoints (List of carla.Transform): List of waypoints (CARLA transforms).
+ _end_transform (carla.Transform): Final position (CARLA transform).
+ _start_time (float): Start time of the atomic [s].
+ Defaults to None.
+ _grp (GlobalPlanner): global planner instance of the town
+ """
+
+ def __init__(self, actor, position, name="ChangeActorWaypointsToReachPosition"):
+ """
+ Setup parameters
+ """
+ super(ChangeActorWaypointsToReachPosition, self).__init__(actor, [])
+
+ self._end_transform = position
+
+ town_map = CarlaDataProvider.get_map()
+ dao = GlobalRoutePlannerDAO(town_map, 2)
+ self._grp = GlobalRoutePlanner(dao)
+ self._grp.setup()
+
+ def initialise(self):
+ """
+ Set _start_time and get (actor, controller) pair from Blackboard.
+
+ Generate a waypoint list (route) which representes the route. Set
+ this waypoint list for the actor controller.
+
+ May throw if actor is not available as key for the ActorsWithController
+ dictionary from Blackboard.
+ """
+
+ # get start position
+ position_actor = CarlaDataProvider.get_location(self._actor)
+
+ # calculate plan with global_route_planner function
+ plan = self._grp.trace_route(position_actor, self._end_transform.location)
+
+ for elem in plan:
+ self._waypoints.append(elem[0].transform)
+
+ super(ChangeActorWaypointsToReachPosition, self).initialise()
+
+
+class ChangeActorLateralMotion(AtomicBehavior):
+
+ """
+ Atomic to change the waypoints for an actor controller.
+
+ The behavior is in RUNNING state until the last waypoint is reached, or if a
+ second waypoint related atomic for the same actor is triggered. These are:
+ - ChangeActorWaypoints
+ - ChangeActorWaypointsToReachPosition
+ - ChangeActorLateralMotion
+
+ Args:
+ actor (carla.Actor): Controlled actor.
+ direction (string): Lane change direction ('left' or 'right').
+ Defaults to 'left'.
+ distance_lane_change (float): Distance of the lance change [meters].
+ Defaults to 25.
+ distance_other_lane (float): Driven distance after the lange change [meters].
+ Defaults to 100.
+ name (string): Name of the behavior.
+ Defaults to 'ChangeActorLateralMotion'.
+
+ Attributes:
+ _waypoints (List of carla.Transform): List of waypoints representing the lane change (CARLA transforms).
+ _direction (string): Lane change direction ('left' or 'right').
+ _distance_same_lane (float): Distance on the same lane before the lane change starts [meters]
+ Constant to 5.
+ _distance_other_lane (float): Max. distance on the target lane after the lance change [meters]
+ Constant to 100.
+ _distance_lane_change (float): Max. total distance of the lane change [meters].
+ _pos_before_lane_change: carla.Location of the actor before the lane change.
+ Defaults to None.
+ _target_lane_id (int): Id of the target lane
+ Defaults to None.
+ _start_time (float): Start time of the atomic [s].
+ Defaults to None.
+ """
+
+ def __init__(self, actor, direction='left', distance_lane_change=25,
+ distance_other_lane=100, name="ChangeActorLateralMotion"):
+ """
+ Setup parameters
+ """
+ super(ChangeActorLateralMotion, self).__init__(name, actor)
+
+ self._waypoints = []
+ self._direction = direction
+ self._distance_same_lane = 5
+ self._distance_other_lane = distance_other_lane
+ self._distance_lane_change = distance_lane_change
+ self._pos_before_lane_change = None
+ self._target_lane_id = None
+
+ self._start_time = None
+
+ def initialise(self):
+ """
+ Set _start_time and get (actor, controller) pair from Blackboard.
+
+ Generate a waypoint list (route) which representes the lane change. Set
+ this waypoint list for the actor controller.
+
+ May throw if actor is not available as key for the ActorsWithController
+ dictionary from Blackboard.
+ """
+ actor_dict = {}
+
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ raise RuntimeError("Actor not found in ActorsWithController BlackBoard")
+
+ self._start_time = GameTime.get_time()
+
+ # get start position
+ position_actor = CarlaDataProvider.get_map().get_waypoint(CarlaDataProvider.get_location(self._actor))
+
+ # calculate plan with scenario_helper function
+ plan, self._target_lane_id = generate_target_waypoint_list_multilane(
+ position_actor, self._direction, self._distance_same_lane,
+ self._distance_other_lane, self._distance_lane_change, check='false')
+
+ for elem in plan:
+ self._waypoints.append(elem[0].transform)
+
+ actor_dict[self._actor.id].update_waypoints(self._waypoints, start_time=self._start_time)
+
+ super(ChangeActorLateralMotion, self).initialise()
+
+ def update(self):
+ """
+ Check the actor's current state and if the lane change was completed
+
+ returns:
+ py_trees.common.Status.SUCCESS, if lane change was successful, or
+ if another ChangeActorLateralMotion atomic for the same actor was triggered.
+ py_trees.common.Status.FAILURE, if the actor is not found in ActorsWithController Blackboard dictionary.
+ py_trees.common.Status.FAILURE, else.
+ """
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if not actor_dict or not self._actor.id in actor_dict:
+ return py_trees.common.Status.FAILURE
+
+ if actor_dict[self._actor.id].get_last_waypoint_command() != self._start_time:
+ return py_trees.common.Status.SUCCESS
+
+ new_status = py_trees.common.Status.RUNNING
+
+ current_position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location())
+ current_lane_id = current_position_actor.lane_id
+
+ if current_lane_id == self._target_lane_id:
+ # driving on new lane
+ distance = current_position_actor.transform.location.distance(self._pos_before_lane_change)
+
+ if distance > self._distance_other_lane:
+ # long enough distance on new lane --> SUCCESS
+ new_status = py_trees.common.Status.SUCCESS
+ else:
+ # no lane change yet
+ self._pos_before_lane_change = current_position_actor.transform.location
+
+ return new_status
+
+
+class ActorTransformSetterToOSCPosition(AtomicBehavior):
+
+ """
+ OpenSCENARIO atomic
+ This class contains an atomic behavior to set the transform of an OpenSCENARIO actor.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - osc_position: OpenSCENARIO position
+ - physics [optional]: If physics is true, the actor physics will be reactivated upon success
+
+ The behavior terminates when actor is set to the new actor transform (closer than 1 meter)
+
+ NOTE:
+ It is very important to ensure that the actor location is spawned to the new transform because of the
+ appearence of a rare runtime processing error. WaypointFollower with LocalPlanner,
+ might fail if new_status is set to success before the actor is really positioned at the new transform.
+ Therefore: calculate_distance(actor, transform) < 1 meter
+ """
+
+ def __init__(self, actor, osc_position, physics=True, name="ActorTransformSetterToOSCPosition"):
+ """
+ Setup parameters
+ """
+ super(ActorTransformSetterToOSCPosition, self).__init__(name, actor)
+ self._osc_position = osc_position
+ self._physics = physics
+ self._osc_transform = None
+
+ def initialise(self):
+
+ super(ActorTransformSetterToOSCPosition, self).initialise()
+
+ if self._actor.is_alive:
+ self._actor.set_target_velocity(carla.Vector3D(0, 0, 0))
+ self._actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0))
+
+ def update(self):
+ """
+ Transform actor
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ # calculate transform with method in openscenario_parser.py
+ self._osc_transform = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
+ self._osc_position)
+ self._actor.set_transform(self._osc_transform)
+
+ if not self._actor.is_alive:
+ new_status = py_trees.common.Status.FAILURE
+
+ if calculate_distance(self._actor.get_location(), self._osc_transform.location) < 1.0:
+ if self._physics:
+ self._actor.set_simulate_physics(enabled=True)
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class AccelerateToVelocity(AtomicBehavior):
+
+ """
+ This class contains an atomic acceleration behavior. The controlled
+ traffic participant will accelerate with _throttle_value_ until reaching
+ a given _target_velocity_
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - throttle_value: The amount of throttle used to accelerate in [0,1]
+ - target_velocity: The target velocity the actor should reach in m/s
+
+ The behavior will terminate, if the actor's velocity is at least target_velocity
+ """
+
+ def __init__(self, actor, throttle_value, target_velocity, name="Acceleration"):
+ """
+ Setup parameters including acceleration value (via throttle_value)
+ and target velocity
+ """
+ super(AccelerateToVelocity, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._control, self._type = get_actor_control(actor)
+ self._throttle_value = throttle_value
+ self._target_velocity = target_velocity
+
+ def initialise(self):
+ # In case of walkers, we have to extract the current heading
+ if self._type == 'walker':
+ self._control.speed = self._target_velocity
+ self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector()
+
+ super(AccelerateToVelocity, self).initialise()
+
+ def update(self):
+ """
+ Set throttle to throttle_value, as long as velocity is < target_velocity
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._type == 'vehicle':
+ if CarlaDataProvider.get_velocity(self._actor) < self._target_velocity:
+ self._control.throttle = self._throttle_value
+ else:
+ new_status = py_trees.common.Status.SUCCESS
+ self._control.throttle = 0
+
+ self._actor.apply_control(self._control)
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class AccelerateToCatchUp(AtomicBehavior):
+
+ """
+ This class contains an atomic acceleration behavior.
+ The car will accelerate until it is faster than another car, in order to catch up distance.
+ This behaviour is especially useful before a lane change (e.g. LaneChange atom).
+
+ Important parameters:
+ - actor: CARLA actor to execute the behaviour
+ - other_actor: Reference CARLA actor, actor you want to catch up to
+ - throttle_value: acceleration value between 0.0 and 1.0
+ - delta_velocity: speed up to the velocity of other actor plus delta_velocity
+ - trigger_distance: distance between the actors
+ - max_distance: driven distance to catch up has to be smaller than max_distance
+
+ The behaviour will terminate succesful, when the two actors are in trigger_distance.
+ If max_distance is driven by the actor before actors are in trigger_distance,
+ then the behaviour ends with a failure.
+ """
+
+ def __init__(self, actor, other_actor, throttle_value=1, delta_velocity=10, trigger_distance=5,
+ max_distance=500, name="AccelerateToCatchUp"):
+ """
+ Setup parameters
+ The target_speet is calculated on the fly.
+ """
+ super(AccelerateToCatchUp, self).__init__(name, actor)
+
+ self._other_actor = other_actor
+ self._throttle_value = throttle_value
+ self._delta_velocity = delta_velocity # 1m/s=3.6km/h
+ self._trigger_distance = trigger_distance
+ self._max_distance = max_distance
+
+ self._control, self._type = get_actor_control(actor)
+
+ self._initial_actor_pos = None
+
+ def initialise(self):
+
+ # get initial actor position
+ self._initial_actor_pos = CarlaDataProvider.get_location(self._actor)
+ super(AccelerateToCatchUp, self).initialise()
+
+ def update(self):
+
+ # get actor speed
+ actor_speed = CarlaDataProvider.get_velocity(self._actor)
+ target_speed = CarlaDataProvider.get_velocity(self._other_actor) + self._delta_velocity
+
+ # distance between actors
+ distance = CarlaDataProvider.get_location(self._actor).distance(
+ CarlaDataProvider.get_location(self._other_actor))
+
+ # driven distance of actor
+ driven_distance = CarlaDataProvider.get_location(self._actor).distance(self._initial_actor_pos)
+
+ if actor_speed < target_speed:
+ # set throttle to throttle_value to accelerate
+ self._control.throttle = self._throttle_value
+
+ if actor_speed >= target_speed:
+ # keep velocity until the actors are in trigger distance
+ self._control.throttle = 0
+
+ self._actor.apply_control(self._control)
+
+ # new status:
+ if distance <= self._trigger_distance:
+ new_status = py_trees.common.Status.SUCCESS
+
+ elif driven_distance > self._max_distance:
+ new_status = py_trees.common.Status.FAILURE
+ else:
+ new_status = py_trees.common.Status.RUNNING
+
+ return new_status
+
+
+class KeepVelocity(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to keep the provided velocity.
+ The controlled traffic participant will accelerate as fast as possible
+ until reaching a given _target_velocity_, which is then maintained for
+ as long as this behavior is active.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - target_velocity: The target velocity the actor should reach
+ - duration[optional]: Duration in seconds of this behavior
+ - distance[optional]: Maximum distance in meters covered by the actor during this behavior
+
+ A termination can be enforced by providing distance or duration values.
+ Alternatively, a parallel termination behavior has to be used.
+ """
+
+ def __init__(self, actor, target_velocity, duration=float("inf"), distance=float("inf"), name="KeepVelocity"):
+ """
+ Setup parameters including acceleration value (via throttle_value)
+ and target velocity
+ """
+ super(KeepVelocity, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._target_velocity = target_velocity
+
+ self._control, self._type = get_actor_control(actor)
+ self._map = self._actor.get_world().get_map()
+ self._waypoint = self._map.get_waypoint(self._actor.get_location())
+
+ self._duration = duration
+ self._target_distance = distance
+ self._distance = 0
+ self._start_time = 0
+ self._location = None
+
+ def initialise(self):
+ self._location = CarlaDataProvider.get_location(self._actor)
+ self._start_time = GameTime.get_time()
+
+ # In case of walkers, we have to extract the current heading
+ if self._type == 'walker':
+ self._control.speed = self._target_velocity
+ self._control.direction = CarlaDataProvider.get_transform(self._actor).get_forward_vector()
+
+ super(KeepVelocity, self).initialise()
+
+ def update(self):
+ """
+ As long as the stop condition (duration or distance) is not violated, set a new vehicle control
+
+ For vehicles: set throttle to throttle_value, as long as velocity is < target_velocity
+ For walkers: simply apply the given self._control
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._type == 'vehicle':
+ if CarlaDataProvider.get_velocity(self._actor) < self._target_velocity:
+ self._control.throttle = 1.0
+ else:
+ self._control.throttle = 0.0
+ self._actor.apply_control(self._control)
+
+ new_location = CarlaDataProvider.get_location(self._actor)
+ self._distance += calculate_distance(self._location, new_location)
+ self._location = new_location
+
+ if self._distance > self._target_distance:
+ new_status = py_trees.common.Status.SUCCESS
+
+ if GameTime.get_time() - self._start_time > self._duration:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ On termination of this behavior, the throttle should be set back to 0.,
+ to avoid further acceleration.
+ """
+
+ if self._type == 'vehicle':
+ self._control.throttle = 0.0
+ elif self._type == 'walker':
+ self._control.speed = 0.0
+ if self._actor is not None and self._actor.is_alive:
+ self._actor.apply_control(self._control)
+ super(KeepVelocity, self).terminate(new_status)
+
+
+class ChangeAutoPilot(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to disable/enable the use of the autopilot.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - activate: True (=enable autopilot) or False (=disable autopilot)
+ - lane_change: Traffic Manager parameter. True (=enable lane changes) or False (=disable lane changes)
+ - distance_between_vehicles: Traffic Manager parameter
+ - max_speed: Traffic Manager parameter. Max speed of the actor. This will only work for road segments
+ with the same speed limit as the first one
+
+ The behavior terminates after changing the autopilot state
+ """
+
+ def __init__(self, actor, activate, parameters=None, name="ChangeAutoPilot"):
+ """
+ Setup parameters
+ """
+ super(ChangeAutoPilot, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._activate = activate
+ self._tm = CarlaDataProvider.get_client().get_trafficmanager(
+ CarlaDataProvider.get_traffic_manager_port())
+ self._parameters = parameters
+
+ def update(self):
+ """
+ De/activate autopilot
+ """
+ self._actor.set_autopilot(self._activate)
+
+ if self._parameters is not None:
+ if "auto_lane_change" in self._parameters:
+ lane_change = self._parameters["auto_lane_change"]
+ self._tm.auto_lane_change(self._actor, lane_change)
+
+ if "max_speed" in self._parameters:
+ max_speed = self._parameters["max_speed"]
+ max_road_speed = self._actor.get_speed_limit()
+ if max_road_speed is not None:
+ percentage = (max_road_speed - max_speed) / max_road_speed * 100.0
+ self._tm.vehicle_percentage_speed_difference(self._actor, percentage)
+ else:
+ print("ChangeAutopilot: Unable to find the vehicle's speed limit")
+
+ if "distance_between_vehicles" in self._parameters:
+ dist_vehicles = self._parameters["distance_between_vehicles"]
+ self._tm.distance_to_leading_vehicle(self._actor, dist_vehicles)
+
+ if "force_lane_change" in self._parameters:
+ force_lane_change = self._parameters["force_lane_change"]
+ self._tm.force_lane_change(self._actor, force_lane_change)
+
+ if "ignore_vehicles_percentage" in self._parameters:
+ ignore_vehicles = self._parameters["ignore_vehicles_percentage"]
+ self._tm.ignore_vehicles_percentage(self._actor, ignore_vehicles)
+
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+
+class StopVehicle(AtomicBehavior):
+
+ """
+ This class contains an atomic stopping behavior. The controlled traffic
+ participant will decelerate with _bake_value_ until reaching a full stop.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - brake_value: Brake value in [0,1] applied
+
+ The behavior terminates when the actor stopped moving
+ """
+
+ def __init__(self, actor, brake_value, name="Stopping"):
+ """
+ Setup _actor and maximum braking value
+ """
+ super(StopVehicle, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._control, self._type = get_actor_control(actor)
+ if self._type == 'walker':
+ self._control.speed = 0
+ self._brake_value = brake_value
+
+ def update(self):
+ """
+ Set brake to brake_value until reaching full stop
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._type == 'vehicle':
+ if CarlaDataProvider.get_velocity(self._actor) > EPSILON:
+ self._control.brake = self._brake_value
+ else:
+ new_status = py_trees.common.Status.SUCCESS
+ self._control.brake = 0
+ else:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self._actor.apply_control(self._control)
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class SyncArrival(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to
+ set velocity of actor so that it reaches location at the same time as
+ actor_reference. The behavior assumes that the two actors are moving
+ towards location in a straight line.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - actor_reference: Reference actor with which arrival is synchronized
+ - target_location: CARLA location where the actors should "meet"
+ - gain[optional]: Coefficient for actor's throttle and break controls
+
+ Note: In parallel to this behavior a termination behavior has to be used
+ to keep continue synchronization for a certain duration, or for a
+ certain distance, etc.
+ """
+
+ def __init__(self, actor, actor_reference, target_location, gain=1, name="SyncArrival"):
+ """
+ Setup required parameters
+ """
+ super(SyncArrival, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._control = carla.VehicleControl()
+ self._actor_reference = actor_reference
+ self._target_location = target_location
+ self._gain = gain
+
+ self._control.steering = 0
+
+ def update(self):
+ """
+ Dynamic control update for actor velocity
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ distance_reference = calculate_distance(CarlaDataProvider.get_location(self._actor_reference),
+ self._target_location)
+ distance = calculate_distance(CarlaDataProvider.get_location(self._actor),
+ self._target_location)
+
+ velocity_reference = CarlaDataProvider.get_velocity(self._actor_reference)
+ time_reference = float('inf')
+ if velocity_reference > 0:
+ time_reference = distance_reference / velocity_reference
+
+ velocity_current = CarlaDataProvider.get_velocity(self._actor)
+ time_current = float('inf')
+ if velocity_current > 0:
+ time_current = distance / velocity_current
+
+ control_value = (self._gain) * (time_current - time_reference)
+
+ if control_value > 0:
+ self._control.throttle = min([control_value, 1])
+ self._control.brake = 0
+ else:
+ self._control.throttle = 0
+ self._control.brake = min([abs(control_value), 1])
+
+ self._actor.apply_control(self._control)
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ On termination of this behavior, the throttle should be set back to 0.,
+ to avoid further acceleration.
+ """
+ if self._actor is not None and self._actor.is_alive:
+ self._control.throttle = 0.0
+ self._control.brake = 0.0
+ self._actor.apply_control(self._control)
+ super(SyncArrival, self).terminate(new_status)
+
+
+class AddNoiseToVehicle(AtomicBehavior):
+
+ """
+ This class contains an atomic jitter behavior.
+ To add noise to steer as well as throttle of the vehicle.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - steer_value: Applied steering noise in [0,1]
+ - throttle_value: Applied throttle noise in [0,1]
+
+ The behavior terminates after setting the new actor controls
+ """
+
+ def __init__(self, actor, steer_value, throttle_value, name="Jittering"):
+ """
+ Setup actor , maximum steer value and throttle value
+ """
+ super(AddNoiseToVehicle, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._control = carla.VehicleControl()
+ self._steer_value = steer_value
+ self._throttle_value = throttle_value
+
+ def update(self):
+ """
+ Set steer to steer_value and throttle to throttle_value until reaching full stop
+ """
+ self._control = self._actor.get_control()
+ self._control.steer = self._steer_value
+ self._control.throttle = self._throttle_value
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ self._actor.apply_control(self._control)
+
+ return new_status
+
+
+class ChangeNoiseParameters(AtomicBehavior):
+
+ """
+ This class contains an atomic jitter behavior.
+ To add noise to steer as well as throttle of the vehicle.
+
+ This behavior should be used in conjuction with AddNoiseToVehicle
+
+ The behavior terminates after one iteration
+ """
+
+ def __init__(self, new_steer_noise, new_throttle_noise,
+ noise_mean, noise_std, dynamic_mean_for_steer, dynamic_mean_for_throttle, name="ChangeJittering"):
+ """
+ Setup actor , maximum steer value and throttle value
+ """
+ super(ChangeNoiseParameters, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._new_steer_noise = new_steer_noise
+ self._new_throttle_noise = new_throttle_noise
+ self._noise_mean = noise_mean
+ self._noise_std = noise_std
+ self._dynamic_mean_for_steer = dynamic_mean_for_steer
+ self._dynamic_mean_for_throttle = dynamic_mean_for_throttle
+
+ self._noise_to_apply = abs(random.gauss(self._noise_mean, self._noise_std))
+
+ def update(self):
+ """
+ Change the noise parameters from the structure copy that it receives.
+ """
+
+ self._new_steer_noise[0] = min(0, -(self._noise_to_apply - self._dynamic_mean_for_steer))
+ self._new_throttle_noise[0] = min(self._noise_to_apply + self._dynamic_mean_for_throttle, 1)
+
+ new_status = py_trees.common.Status.SUCCESS
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+
+class BasicAgentBehavior(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior, which uses the
+ basic_agent from CARLA to control the actor until
+ reaching a target location.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - target_location: Is the desired target location (carla.location),
+ the actor should move to
+
+ The behavior terminates after reaching the target_location (within 2 meters)
+ """
+
+ _acceptable_target_distance = 2
+
+ def __init__(self, actor, target_location, name="BasicAgentBehavior"):
+ """
+ Setup actor and maximum steer value
+ """
+ super(BasicAgentBehavior, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._agent = BasicAgent(actor) # pylint: disable=undefined-variable
+ self._agent.set_destination((target_location.x, target_location.y, target_location.z))
+ self._control = carla.VehicleControl()
+ self._target_location = target_location
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+
+ self._control = self._agent.run_step()
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if calculate_distance(location, self._target_location) < self._acceptable_target_distance:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ self._actor.apply_control(self._control)
+
+ return new_status
+
+ def terminate(self, new_status):
+ self._control.throttle = 0.0
+ self._control.brake = 0.0
+ self._actor.apply_control(self._control)
+ super(BasicAgentBehavior, self).terminate(new_status)
+
+
+class Idle(AtomicBehavior):
+
+ """
+ This class contains an idle behavior scenario
+
+ Important parameters:
+ - duration[optional]: Duration in seconds of this behavior
+
+ A termination can be enforced by providing a duration value.
+ Alternatively, a parallel termination behavior has to be used.
+ """
+
+ def __init__(self, duration=float("inf"), name="Idle"):
+ """
+ Setup actor
+ """
+ super(Idle, self).__init__(name)
+ self._duration = duration
+ self._start_time = 0
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def initialise(self):
+ """
+ Set start time
+ """
+ self._start_time = GameTime.get_time()
+ super(Idle, self).initialise()
+
+ def update(self):
+ """
+ Keep running until termination condition is satisfied
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if GameTime.get_time() - self._start_time > self._duration:
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class WaypointFollower(AtomicBehavior):
+
+ """
+ This is an atomic behavior to follow waypoints while maintaining a given speed.
+ If no plan is provided, the actor will follow its foward waypoints indefinetely.
+ Otherwise, the behavior will end with SUCCESS upon reaching the end of the plan.
+ If no target velocity is provided, the actor continues with its current velocity.
+
+ Args:
+ actor (carla.Actor): CARLA actor to execute the behavior.
+ target_speed (float, optional): Desired speed of the actor in m/s. Defaults to None.
+ plan ([carla.Location] or [(carla.Waypoint, carla.agent.navigation.local_planner)], optional):
+ Waypoint plan the actor should follow. Defaults to None.
+ blackboard_queue_name (str, optional):
+ Blackboard variable name, if additional actors should be created on-the-fly. Defaults to None.
+ avoid_collision (bool, optional):
+ Enable/Disable(=default) collision avoidance for vehicles/bikes. Defaults to False.
+ name (str, optional): Name of the behavior. Defaults to "FollowWaypoints".
+
+ Attributes:
+ actor (carla.Actor): CARLA actor to execute the behavior.
+ name (str, optional): Name of the behavior.
+ _target_speed (float, optional): Desired speed of the actor in m/s. Defaults to None.
+ _plan ([carla.Location] or [(carla.Waypoint, carla.agent.navigation.local_planner)]):
+ Waypoint plan the actor should follow. Defaults to None.
+ _blackboard_queue_name (str):
+ Blackboard variable name, if additional actors should be created on-the-fly. Defaults to None.
+ _avoid_collision (bool): Enable/Disable(=default) collision avoidance for vehicles/bikes. Defaults to False.
+ _actor_dict: Dictonary of all actors, and their corresponding plans (e.g. {actor: plan}).
+ _local_planner_dict: Dictonary of all actors, and their corresponding local planners.
+ Either "Walker" for pedestrians, or a carla.agent.navigation.LocalPlanner for other actors.
+ _args_lateral_dict: Parameters for the PID of the used carla.agent.navigation.LocalPlanner.
+ _unique_id: Unique ID of the behavior based on timestamp in nanoseconds.
+
+ Note:
+ OpenScenario:
+ The WaypointFollower atomic must be called with an individual name if multiple consecutive WFs.
+ Blackboard variables with lists are used for consecutive WaypointFollower behaviors.
+ Termination of active WaypointFollowers in initialise of AtomicBehavior because any
+ following behavior must terminate the WaypointFollower.
+ """
+
+ def __init__(self, actor, target_speed=None, plan=None, blackboard_queue_name=None,
+ avoid_collision=False, name="FollowWaypoints"):
+ """
+ Set up actor and local planner
+ """
+ super(WaypointFollower, self).__init__(name, actor)
+ self._actor_dict = {}
+ self._actor_dict[actor] = None
+ self._target_speed = target_speed
+ self._local_planner_dict = {}
+ self._local_planner_dict[actor] = None
+ self._plan = plan
+ self._blackboard_queue_name = blackboard_queue_name
+ if blackboard_queue_name is not None:
+ self._queue = Blackboard().get(blackboard_queue_name)
+ self._args_lateral_dict = {'K_P': 1.0, 'K_D': 0.01, 'K_I': 0.0, 'dt': 0.05}
+ self._avoid_collision = avoid_collision
+ self._unique_id = 0
+
+ def initialise(self):
+ """
+ Delayed one-time initialization
+
+ Checks if another WaypointFollower behavior is already running for this actor.
+ If this is the case, a termination signal is sent to the running behavior.
+ """
+ super(WaypointFollower, self).initialise()
+ self._unique_id = int(round(time.time() * 1e9))
+ try:
+ # check whether WF for this actor is already running and add new WF to running_WF list
+ check_attr = operator.attrgetter("running_WF_actor_{}".format(self._actor.id))
+ running = check_attr(py_trees.blackboard.Blackboard())
+ active_wf = copy.copy(running)
+ active_wf.append(self._unique_id)
+ py_trees.blackboard.Blackboard().set(
+ "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True)
+ except AttributeError:
+ # no WF is active for this actor
+ py_trees.blackboard.Blackboard().set("terminate_WF_actor_{}".format(self._actor.id), [], overwrite=True)
+ py_trees.blackboard.Blackboard().set(
+ "running_WF_actor_{}".format(self._actor.id), [self._unique_id], overwrite=True)
+
+ for actor in self._actor_dict:
+ self._apply_local_planner(actor)
+ return True
+
+ def _apply_local_planner(self, actor):
+ """
+ Convert the plan into locations for walkers (pedestrians), or to a waypoint list for other actors.
+ For non-walkers, activate the carla.agent.navigation.LocalPlanner module.
+ """
+ if self._target_speed is None:
+ self._target_speed = CarlaDataProvider.get_velocity(actor)
+ else:
+ self._target_speed = self._target_speed
+
+ if isinstance(actor, carla.Walker):
+ self._local_planner_dict[actor] = "Walker"
+ if self._plan is not None:
+ if isinstance(self._plan[0], carla.Location):
+ self._actor_dict[actor] = self._plan
+ else:
+ self._actor_dict[actor] = [element[0].transform.location for element in self._plan]
+ else:
+ local_planner = LocalPlanner( # pylint: disable=undefined-variable
+ actor, opt_dict={
+ 'target_speed': self._target_speed * 3.6,
+ 'lateral_control_dict': self._args_lateral_dict})
+
+ if self._plan is not None:
+ if isinstance(self._plan[0], carla.Location):
+ plan = []
+ for location in self._plan:
+ waypoint = CarlaDataProvider.get_map().get_waypoint(location,
+ project_to_road=True,
+ lane_type=carla.LaneType.Any)
+ plan.append((waypoint, RoadOption.LANEFOLLOW))
+ local_planner.set_global_plan(plan)
+ else:
+ local_planner.set_global_plan(self._plan)
+
+ self._local_planner_dict[actor] = local_planner
+ self._actor_dict[actor] = self._plan
+
+ def update(self):
+ """
+ Compute next control step for the given waypoint plan, obtain and apply control to actor
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ check_term = operator.attrgetter("terminate_WF_actor_{}".format(self._actor.id))
+ terminate_wf = check_term(py_trees.blackboard.Blackboard())
+
+ check_run = operator.attrgetter("running_WF_actor_{}".format(self._actor.id))
+ active_wf = check_run(py_trees.blackboard.Blackboard())
+
+ # Termination of WF if the WFs unique_id is listed in terminate_wf
+ # only one WF should be active, therefore all previous WF have to be terminated
+ if self._unique_id in terminate_wf:
+ terminate_wf.remove(self._unique_id)
+ if self._unique_id in active_wf:
+ active_wf.remove(self._unique_id)
+
+ py_trees.blackboard.Blackboard().set(
+ "terminate_WF_actor_{}".format(self._actor.id), terminate_wf, overwrite=True)
+ py_trees.blackboard.Blackboard().set(
+ "running_WF_actor_{}".format(self._actor.id), active_wf, overwrite=True)
+ new_status = py_trees.common.Status.SUCCESS
+ return new_status
+
+ if self._blackboard_queue_name is not None:
+ while not self._queue.empty():
+ actor = self._queue.get()
+ if actor is not None and actor not in self._actor_dict:
+ self._apply_local_planner(actor)
+
+ success = True
+ for actor in self._local_planner_dict:
+ local_planner = self._local_planner_dict[actor] if actor else None
+ if actor is not None and actor.is_alive and local_planner is not None:
+ # Check if the actor is a vehicle/bike
+ if not isinstance(actor, carla.Walker):
+ control = local_planner.run_step(debug=False)
+ if self._avoid_collision and detect_lane_obstacle(actor):
+ control.throttle = 0.0
+ control.brake = 1.0
+ actor.apply_control(control)
+ # Check if the actor reached the end of the plan
+ # @TODO replace access to private _waypoints_queue with public getter
+ if local_planner._waypoints_queue: # pylint: disable=protected-access
+ success = False
+ # If the actor is a pedestrian, we have to use the WalkerAIController
+ # The walker is sent to the next waypoint in its plan
+ else:
+ actor_location = CarlaDataProvider.get_location(actor)
+ success = False
+ if self._actor_dict[actor]:
+ location = self._actor_dict[actor][0]
+ direction = location - actor_location
+ direction_norm = math.sqrt(direction.x**2 + direction.y**2)
+ control = actor.get_control()
+ control.speed = self._target_speed
+ control.direction = direction / direction_norm
+ actor.apply_control(control)
+ if direction_norm < 1.0:
+ self._actor_dict[actor] = self._actor_dict[actor][1:]
+ if self._actor_dict[actor] is None:
+ success = True
+ else:
+ control = actor.get_control()
+ control.speed = self._target_speed
+ control.direction = CarlaDataProvider.get_transform(actor).rotation.get_forward_vector()
+ actor.apply_control(control)
+
+ if success:
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ On termination of this behavior,
+ the controls should be set back to 0.
+ """
+ for actor in self._local_planner_dict:
+ if actor is not None and actor.is_alive:
+ control, _ = get_actor_control(actor)
+ actor.apply_control(control)
+ local_planner = self._local_planner_dict[actor]
+ if local_planner is not None and local_planner != "Walker":
+ local_planner.reset_vehicle()
+ local_planner = None
+
+ self._local_planner_dict = {}
+ self._actor_dict = {}
+ super(WaypointFollower, self).terminate(new_status)
+
+
+class LaneChange(WaypointFollower):
+
+ """
+ This class inherits from the class WaypointFollower.
+
+ This class contains an atomic lane change behavior to a parallel lane.
+ The vehicle follows a waypoint plan to the other lane, which is calculated in the initialise method.
+ This waypoint plan is calculated with a scenario helper function.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - speed: speed of the actor for the lane change, in m/s
+ - direction: 'right' or 'left', depending on which lane to change
+ - distance_same_lane: straight distance before lane change, in m
+ - distance_other_lane: straight distance after lane change, in m
+ - distance_lane_change: straight distance for the lane change itself, in m
+
+ The total distance driven is greater than the sum of distance_same_lane and distance_other_lane.
+ It results from the lane change distance plus the distance_same_lane plus distance_other_lane.
+ The lane change distance is set to 25m (straight), the driven distance is slightly greater.
+
+ A parallel termination behavior has to be used.
+ """
+
+ def __init__(self, actor, speed=10, direction='left',
+ distance_same_lane=5, distance_other_lane=100, distance_lane_change=25, name='LaneChange'):
+
+ self._direction = direction
+ self._distance_same_lane = distance_same_lane
+ self._distance_other_lane = distance_other_lane
+ self._distance_lane_change = distance_lane_change
+
+ self._target_lane_id = None
+ self._distance_new_lane = 0
+ self._pos_before_lane_change = None
+
+ super(LaneChange, self).__init__(actor, target_speed=speed, name=name)
+
+ def initialise(self):
+
+ # get start position
+ position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location())
+
+ # calculate plan with scenario_helper function
+ self._plan, self._target_lane_id = generate_target_waypoint_list_multilane(
+ position_actor, self._direction, self._distance_same_lane,
+ self._distance_other_lane, self._distance_lane_change, check='true')
+ super(LaneChange, self).initialise()
+
+ def update(self):
+ status = super(LaneChange, self).update()
+
+ current_position_actor = CarlaDataProvider.get_map().get_waypoint(self._actor.get_location())
+ current_lane_id = current_position_actor.lane_id
+
+ if current_lane_id == self._target_lane_id:
+ # driving on new lane
+ distance = current_position_actor.transform.location.distance(self._pos_before_lane_change)
+
+ if distance > self._distance_other_lane:
+ # long enough distance on new lane --> SUCCESS
+ status = py_trees.common.Status.SUCCESS
+ else:
+ # no lane change yet
+ self._pos_before_lane_change = current_position_actor.transform.location
+
+ return status
+
+
+class SetInitSpeed(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to set the init_speed of an actor,
+ succeding immeditely after initializing
+ """
+
+ def __init__(self, actor, init_speed=10, name='SetInitSpeed'):
+
+ self._init_speed = init_speed
+ self._terminate = None
+ self._actor = actor
+
+ super(SetInitSpeed, self).__init__(name, actor)
+
+ def initialise(self):
+ """
+ Initialize it's speed
+ """
+
+ transform = self._actor.get_transform()
+ yaw = transform.rotation.yaw * (math.pi / 180)
+
+ vx = math.cos(yaw) * self._init_speed
+ vy = math.sin(yaw) * self._init_speed
+ self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
+
+ def update(self):
+ """
+ Nothing to update, end the behavior
+ """
+
+ return py_trees.common.Status.SUCCESS
+
+
+class HandBrakeVehicle(AtomicBehavior):
+
+ """
+ This class contains an atomic hand brake behavior.
+ To set the hand brake value of the vehicle.
+
+ Important parameters:
+ - vehicle: CARLA actor to execute the behavior
+ - hand_brake_value to be applied in [0,1]
+
+ The behavior terminates after setting the hand brake value
+ """
+
+ def __init__(self, vehicle, hand_brake_value, name="Braking"):
+ """
+ Setup vehicle control and brake value
+ """
+ super(HandBrakeVehicle, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._vehicle = vehicle
+ self._control, self._type = get_actor_control(vehicle)
+ self._hand_brake_value = hand_brake_value
+
+ def update(self):
+ """
+ Set handbrake
+ """
+ new_status = py_trees.common.Status.SUCCESS
+ if self._type == 'vehicle':
+ self._control.hand_brake = self._hand_brake_value
+ self._vehicle.apply_control(self._control)
+ else:
+ self._hand_brake_value = None
+ self.logger.debug("%s.update()[%s->%s]" %
+ (self.__class__.__name__, self.status, new_status))
+ self._vehicle.apply_control(self._control)
+
+ return new_status
+
+
+class ActorDestroy(AtomicBehavior):
+
+ """
+ This class contains an actor destroy behavior.
+ Given an actor this behavior will delete it.
+
+ Important parameters:
+ - actor: CARLA actor to be deleted
+
+ The behavior terminates after removing the actor
+ """
+
+ def __init__(self, actor, name="ActorDestroy"):
+ """
+ Setup actor
+ """
+ super(ActorDestroy, self).__init__(name, actor)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+ if self._actor:
+ CarlaDataProvider.remove_actor_by_id(self._actor.id)
+ self._actor = None
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class ActorTransformSetter(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to set the transform
+ of an actor.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - transform: New target transform (position + orientation) of the actor
+ - physics [optional]: If physics is true, the actor physics will be reactivated upon success
+
+ The behavior terminates when actor is set to the new actor transform (closer than 1 meter)
+
+ NOTE:
+ It is very important to ensure that the actor location is spawned to the new transform because of the
+ appearence of a rare runtime processing error. WaypointFollower with LocalPlanner,
+ might fail if new_status is set to success before the actor is really positioned at the new transform.
+ Therefore: calculate_distance(actor, transform) < 1 meter
+ """
+
+ def __init__(self, actor, transform, physics=True, name="ActorTransformSetter"):
+ """
+ Init
+ """
+ super(ActorTransformSetter, self).__init__(name, actor)
+ self._transform = transform
+ self._physics = physics
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def initialise(self):
+ if self._actor.is_alive:
+ self._actor.set_target_velocity(carla.Vector3D(0, 0, 0))
+ self._actor.set_target_angular_velocity(carla.Vector3D(0, 0, 0))
+ self._actor.set_transform(self._transform)
+ super(ActorTransformSetter, self).initialise()
+
+ def update(self):
+ """
+ Transform actor
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if not self._actor.is_alive:
+ new_status = py_trees.common.Status.FAILURE
+
+ if calculate_distance(self._actor.get_location(), self._transform.location) < 1.0:
+ if self._physics:
+ self._actor.set_simulate_physics(enabled=True)
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class TrafficLightStateSetter(AtomicBehavior):
+
+ """
+ This class contains an atomic behavior to set the state of a given traffic light
+
+ Args:
+ actor (carla.TrafficLight): ID of the traffic light that shall be changed
+ state (carla.TrafficLightState): New target state
+
+ The behavior terminates after trying to set the new state
+ """
+
+ def __init__(self, actor, state, name="TrafficLightStateSetter"):
+ """
+ Init
+ """
+ super(TrafficLightStateSetter, self).__init__(name)
+
+ self._actor = actor if "traffic_light" in actor.type_id else None
+ self._state = state
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+ """
+ Change the state of the traffic light
+ """
+ if self._actor is None:
+ return py_trees.common.Status.FAILURE
+
+ new_status = py_trees.common.Status.RUNNING
+ if self._actor.is_alive:
+ self._actor.set_state(self._state)
+ new_status = py_trees.common.Status.SUCCESS
+ else:
+ # For some reason the actor is gone...
+ new_status = py_trees.common.Status.FAILURE
+
+ return new_status
+
+
+class ActorSource(AtomicBehavior):
+
+ """
+ Implementation for a behavior that will indefinitely create actors
+ at a given transform if no other actor exists in a given radius
+ from the transform.
+
+ Important parameters:
+ - actor_type_list: Type of CARLA actors to be spawned
+ - transform: Spawn location
+ - threshold: Min available free distance between other actors and the spawn location
+ - blackboard_queue_name: Name of the blackboard used to control this behavior
+ - actor_limit [optional]: Maximum number of actors to be spawned (default=7)
+
+ A parallel termination behavior has to be used.
+ """
+
+ def __init__(self, actor_type_list, transform, threshold, blackboard_queue_name,
+ actor_limit=7, name="ActorSource"):
+ """
+ Setup class members
+ """
+ super(ActorSource, self).__init__(name)
+ self._world = CarlaDataProvider.get_world()
+ self._actor_types = actor_type_list
+ self._spawn_point = transform
+ self._threshold = threshold
+ self._queue = Blackboard().get(blackboard_queue_name)
+ self._actor_limit = actor_limit
+ self._last_blocking_actor = None
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+ if self._actor_limit > 0:
+ world_actors = self._world.get_actors()
+ spawn_point_blocked = False
+ if (self._last_blocking_actor and
+ self._spawn_point.location.distance(self._last_blocking_actor.get_location()) < self._threshold):
+ spawn_point_blocked = True
+
+ if not spawn_point_blocked:
+ for actor in world_actors:
+ if self._spawn_point.location.distance(actor.get_location()) < self._threshold:
+ spawn_point_blocked = True
+ self._last_blocking_actor = actor
+ break
+
+ if not spawn_point_blocked:
+ try:
+ new_actor = CarlaDataProvider.request_new_actor(
+ np.random.choice(self._actor_types), self._spawn_point)
+ self._actor_limit -= 1
+ self._queue.put(new_actor)
+ except: # pylint: disable=bare-except
+ print("ActorSource unable to spawn actor")
+ return new_status
+
+
+class ActorSink(AtomicBehavior):
+
+ """
+ Implementation for a behavior that will indefinitely destroy actors
+ that wander near a given location within a specified threshold.
+
+ Important parameters:
+ - actor_type_list: Type of CARLA actors to be spawned
+ - sink_location: Location (carla.location) at which actors will be deleted
+ - threshold: Distance around sink_location in which actors will be deleted
+
+ A parallel termination behavior has to be used.
+ """
+
+ def __init__(self, sink_location, threshold, name="ActorSink"):
+ """
+ Setup class members
+ """
+ super(ActorSink, self).__init__(name)
+ self._sink_location = sink_location
+ self._threshold = threshold
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+ CarlaDataProvider.remove_actors_in_surrounding(self._sink_location, self._threshold)
+ return new_status
+
+
+class StartRecorder(AtomicBehavior):
+
+ """
+ Atomic that starts the CARLA recorder. Only one can be active
+ at a time, and if this isn't the case, the recorder will
+ automatically stop the previous one.
+
+ Args:
+ recorder_name (str): name of the file to write the recorded data.
+ Remember that a simple name will save the recording in
+ 'CarlaUE4/Saved/'. Otherwise, if some folder appears in the name,
+ it will be considered an absolute path.
+ name (str): name of the behavior
+ """
+
+ def __init__(self, recorder_name, name="StartRecorder"):
+ """
+ Setup class members
+ """
+ super(StartRecorder, self).__init__(name)
+ self._client = CarlaDataProvider.get_client()
+ self._recorder_name = recorder_name
+
+ def update(self):
+ self._client.start_recorder(self._recorder_name)
+ return py_trees.common.Status.SUCCESS
+
+
+class StopRecorder(AtomicBehavior):
+
+ """
+ Atomic that stops the CARLA recorder.
+
+ Args:
+ name (str): name of the behavior
+ """
+
+ def __init__(self, name="StopRecorder"):
+ """
+ Setup class members
+ """
+ super(StopRecorder, self).__init__(name)
+ self._client = CarlaDataProvider.get_client()
+
+ def update(self):
+ self._client.stop_recorder()
+ return py_trees.common.Status.SUCCESS
+
+
+class TrafficLightManipulator(AtomicBehavior):
+
+ """
+ Atomic behavior that manipulates traffic lights around the ego_vehicle to trigger scenarios 7 to 10.
+ This is done by setting 2 of the traffic light at the intersection to green (with some complex precomputation
+ to set everything up).
+
+ Important parameters:
+ - ego_vehicle: CARLA actor that controls this behavior
+ - subtype: string that gathers information of the route and scenario number
+ (check SUBTYPE_CONFIG_TRANSLATION below)
+ """
+
+ RED = carla.TrafficLightState.Red
+ YELLOW = carla.TrafficLightState.Yellow
+ GREEN = carla.TrafficLightState.Green
+
+ # Time constants
+ RED_TIME = 1.5 # Minimum time the ego vehicle waits in red (seconds)
+ YELLOW_TIME = 2 # Time spent at yellow state (seconds)
+ RESET_TIME = 6 # Time waited before resetting all the junction (seconds)
+
+ # Experimental values
+ TRIGGER_DISTANCE = 10 # Distance that makes all vehicles in the lane enter the junction (meters)
+ DIST_TO_WAITING_TIME = 0.04 # Used to wait longer at larger intersections (s/m)
+
+ INT_CONF_OPP1 = {'ego': RED, 'ref': RED, 'left': RED, 'right': RED, 'opposite': GREEN}
+ INT_CONF_OPP2 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': RED, 'opposite': GREEN}
+ INT_CONF_LFT1 = {'ego': RED, 'ref': RED, 'left': GREEN, 'right': RED, 'opposite': RED}
+ INT_CONF_LFT2 = {'ego': GREEN, 'ref': GREEN, 'left': GREEN, 'right': RED, 'opposite': RED}
+ INT_CONF_RGT1 = {'ego': RED, 'ref': RED, 'left': RED, 'right': GREEN, 'opposite': RED}
+ INT_CONF_RGT2 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': GREEN, 'opposite': RED}
+
+ INT_CONF_REF1 = {'ego': GREEN, 'ref': GREEN, 'left': RED, 'right': RED, 'opposite': RED}
+ INT_CONF_REF2 = {'ego': YELLOW, 'ref': YELLOW, 'left': RED, 'right': RED, 'opposite': RED}
+
+ # Depending on the scenario, IN ORDER OF IMPORTANCE, the traffic light changed
+ # The list has to contain only items of the INT_CONF
+ SUBTYPE_CONFIG_TRANSLATION = {
+ 'S7left': ['left', 'opposite', 'right'],
+ 'S7right': ['left', 'opposite'],
+ 'S7opposite': ['right', 'left', 'opposite'],
+ 'S8left': ['opposite'],
+ 'S9right': ['left', 'opposite']
+ }
+
+ CONFIG_TLM_TRANSLATION = {
+ 'left': [INT_CONF_LFT1, INT_CONF_LFT2],
+ 'right': [INT_CONF_RGT1, INT_CONF_RGT2],
+ 'opposite': [INT_CONF_OPP1, INT_CONF_OPP2]
+ }
+
+ def __init__(self, ego_vehicle, subtype, debug=False, name="TrafficLightManipulator"):
+ super(TrafficLightManipulator, self).__init__(name)
+ self.ego_vehicle = ego_vehicle
+ self.subtype = subtype
+ self.current_step = 1
+ self.debug = debug
+
+ self.traffic_light = None
+ self.annotations = None
+ self.configuration = None
+ self.prev_junction_state = None
+ self.junction_location = None
+ self.seconds_waited = 0
+ self.prev_time = None
+ self.max_trigger_distance = None
+ self.waiting_time = None
+ self.inside_junction = False
+
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+
+ new_status = py_trees.common.Status.RUNNING
+
+ # 1) Set up the parameters
+ if self.current_step == 1:
+
+ # Traffic light affecting the ego vehicle
+ self.traffic_light = CarlaDataProvider.get_next_traffic_light(self.ego_vehicle, use_cached_location=False)
+ if not self.traffic_light:
+ # nothing else to do in this iteration...
+ return new_status
+
+ # "Topology" of the intersection
+ self.annotations = CarlaDataProvider.annotate_trafficlight_in_group(self.traffic_light)
+
+ # Which traffic light will be modified (apart from the ego lane)
+ self.configuration = self.get_traffic_light_configuration(self.subtype, self.annotations)
+ if self.configuration is None:
+ self.current_step = 0 # End the behavior
+ return new_status
+
+ # Modify the intersection. Store the previous state
+ self.prev_junction_state = self.set_intersection_state(self.INT_CONF_REF1)
+
+ self.current_step += 1
+ if self.debug:
+ print("--- All set up")
+
+ # 2) Modify the ego lane to yellow when closeby
+ elif self.current_step == 2:
+
+ ego_location = CarlaDataProvider.get_location(self.ego_vehicle)
+
+ if self.junction_location is None:
+ ego_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location)
+ junction_waypoint = ego_waypoint.next(0.5)[0]
+ while not junction_waypoint.is_junction:
+ next_wp = junction_waypoint.next(0.5)[0]
+ junction_waypoint = next_wp
+ self.junction_location = junction_waypoint.transform.location
+
+ distance = ego_location.distance(self.junction_location)
+
+ # Failure check
+ if self.max_trigger_distance is None:
+ self.max_trigger_distance = distance + 1
+ if distance > self.max_trigger_distance:
+ self.current_step = 0
+
+ elif distance < self.TRIGGER_DISTANCE:
+ _ = self.set_intersection_state(self.INT_CONF_REF2)
+ self.current_step += 1
+
+ if self.debug:
+ print("--- Distance until traffic light changes: {}".format(distance))
+
+ # 3) Modify the ego lane to red and the chosen one to green after several seconds
+ elif self.current_step == 3:
+
+ if self.passed_enough_time(self.YELLOW_TIME):
+ _ = self.set_intersection_state(self.CONFIG_TLM_TRANSLATION[self.configuration][0])
+
+ self.current_step += 1
+
+ # 4) Wait a bit to let vehicles enter the intersection, then set the ego lane to green
+ elif self.current_step == 4:
+
+ # Get the time in red, dependent on the intersection dimensions
+ if self.waiting_time is None:
+ self.waiting_time = self.get_waiting_time(self.annotations, self.configuration)
+
+ if self.passed_enough_time(self.waiting_time):
+ _ = self.set_intersection_state(self.CONFIG_TLM_TRANSLATION[self.configuration][1])
+
+ self.current_step += 1
+
+ # 5) Wait for the end of the intersection
+ elif self.current_step == 5:
+ # the traffic light has been manipulated, wait until the vehicle finsihes the intersection
+ ego_location = CarlaDataProvider.get_location(self.ego_vehicle)
+ ego_waypoint = CarlaDataProvider.get_map().get_waypoint(ego_location)
+
+ if not self.inside_junction:
+ if ego_waypoint.is_junction:
+ # Wait for the ego_vehicle to enter a junction
+ self.inside_junction = True
+ else:
+ if self.debug:
+ print("--- Waiting to ENTER a junction")
+
+ else:
+ if ego_waypoint.is_junction:
+ if self.debug:
+ print("--- Waiting to EXIT a junction")
+ else:
+ # And to leave it
+ self.inside_junction = False
+ self.current_step += 1
+
+ # 6) At the end (or if something failed), reset to the previous state
+ else:
+ if self.prev_junction_state:
+ CarlaDataProvider.reset_lights(self.prev_junction_state)
+ if self.debug:
+ print("--- Returning the intersection to its previous state")
+
+ self.variable_cleanup()
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+ def passed_enough_time(self, time_limit):
+ """
+ Returns true or false depending on the time that has passed from the
+ first time this function was called
+ """
+ # Start the timer
+ if self.prev_time is None:
+ self.prev_time = GameTime.get_time()
+
+ timestamp = GameTime.get_time()
+ self.seconds_waited += (timestamp - self.prev_time)
+ self.prev_time = timestamp
+
+ if self.debug:
+ print("--- Waited seconds: {}".format(self.seconds_waited))
+
+ if self.seconds_waited >= time_limit:
+ self.seconds_waited = 0
+ self.prev_time = None
+
+ return True
+ return False
+
+ def set_intersection_state(self, choice):
+ """
+ Changes the intersection to the desired state
+ """
+ prev_state = CarlaDataProvider.update_light_states(
+ self.traffic_light,
+ self.annotations,
+ choice,
+ freeze=True)
+
+ return prev_state
+
+ def get_waiting_time(self, annotation, direction):
+ """
+ Calculates the time the ego traffic light will remain red
+ to let vehicles enter the junction
+ """
+
+ tl = annotation[direction][0]
+ ego_tl = annotation["ref"][0]
+
+ tl_location = CarlaDataProvider.get_trafficlight_trigger_location(tl)
+ ego_tl_location = CarlaDataProvider.get_trafficlight_trigger_location(ego_tl)
+
+ distance = ego_tl_location.distance(tl_location)
+
+ return self.RED_TIME + distance * self.DIST_TO_WAITING_TIME
+
+ def get_traffic_light_configuration(self, subtype, annotations):
+ """
+ Checks the list of possible altered traffic lights and gets
+ the first one that exists in the intersection
+
+ Important parameters:
+ - subtype: Subtype of the scenario
+ - annotations: list of the traffic light of the junction, with their direction (right, left...)
+ """
+ configuration = None
+
+ if subtype in self.SUBTYPE_CONFIG_TRANSLATION:
+ possible_configurations = self.SUBTYPE_CONFIG_TRANSLATION[self.subtype]
+ while possible_configurations:
+ # Chose the first one and delete it
+ configuration = possible_configurations[0]
+ possible_configurations = possible_configurations[1:]
+ if configuration in annotations:
+ if annotations[configuration]:
+ # Found a valid configuration
+ break
+ else:
+ # The traffic light doesn't exist, get another one
+ configuration = None
+ else:
+ if self.debug:
+ print("This configuration name is wrong")
+ configuration = None
+
+ if configuration is None and self.debug:
+ print("This subtype has no traffic light available")
+ else:
+ if self.debug:
+ print("This subtype is unknown")
+
+ return configuration
+
+ def variable_cleanup(self):
+ """
+ Resets all variables to the intial state
+ """
+ self.current_step = 1
+ self.traffic_light = None
+ self.annotations = None
+ self.configuration = None
+ self.prev_junction_state = None
+ self.junction_location = None
+ self.max_trigger_distance = None
+ self.waiting_time = None
+ self.inside_junction = False
+
+
+class ScenarioTriggerer(AtomicBehavior):
+
+ """
+ Handles the triggering of the scenarios that are part of a route.
+
+ Initializes a list of blackboard variables to False, and only sets them to True when
+ the ego vehicle is very close to the scenarios
+ """
+
+ WINDOWS_SIZE = 5
+
+ def __init__(self, actor, route, blackboard_list, distance,
+ repeat_scenarios=False, debug=False, name="ScenarioTriggerer"):
+ """
+ Setup class members
+ """
+ super(ScenarioTriggerer, self).__init__(name)
+ self._world = CarlaDataProvider.get_world()
+ self._map = CarlaDataProvider.get_map()
+ self._repeat = repeat_scenarios
+ self._debug = debug
+
+ self._actor = actor
+ self._route = route
+ self._distance = distance
+ self._blackboard_list = blackboard_list
+ self._triggered_scenarios = [] # List of already done scenarios
+
+ self._current_index = 0
+ self._route_length = len(self._route)
+ self._waypoints, _ = zip(*self._route)
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ lower_bound = self._current_index
+ upper_bound = min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)
+
+ shortest_distance = float('inf')
+ closest_index = -1
+
+ for index in range(lower_bound, upper_bound):
+ ref_waypoint = self._waypoints[index]
+ ref_location = ref_waypoint.location
+
+ dist_to_route = ref_location.distance(location)
+ if dist_to_route <= shortest_distance:
+ closest_index = index
+ shortest_distance = dist_to_route
+
+ if closest_index == -1 or shortest_distance == float('inf'):
+ return new_status
+
+ # Update the ego position at the route
+ self._current_index = closest_index
+
+ route_location = self._waypoints[closest_index].location
+
+ # Check which scenarios can be triggered
+ blackboard = py_trees.blackboard.Blackboard()
+ for black_var_name, scen_location in self._blackboard_list:
+
+ # Close enough
+ scen_distance = route_location.distance(scen_location)
+ condition1 = bool(scen_distance < self._distance)
+
+ # Not being currently done
+ value = blackboard.get(black_var_name)
+ condition2 = bool(not value)
+
+ # Already done, if needed
+ condition3 = bool(self._repeat or black_var_name not in self._triggered_scenarios)
+
+ if condition1 and condition2 and condition3:
+ _ = blackboard.set(black_var_name, True)
+ self._triggered_scenarios.append(black_var_name)
+
+ if self._debug:
+ self._world.debug.draw_point(
+ scen_location + carla.Location(z=4),
+ size=0.5,
+ life_time=0.5,
+ color=carla.Color(255, 255, 0)
+ )
+ self._world.debug.draw_string(
+ scen_location + carla.Location(z=5),
+ str(black_var_name),
+ False,
+ color=carla.Color(0, 0, 0),
+ life_time=1000
+ )
+
+ return new_status
diff --git a/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
new file mode 100644
index 0000000..f611390
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_criteria.py
@@ -0,0 +1,2048 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides all atomic evaluation criteria required to analyze if a
+scenario was completed successfully or failed.
+
+Criteria should run continuously to monitor the state of a single actor, multiple
+actors or environmental parameters. Hence, a termination is not required.
+
+The atomic criteria are implemented with py_trees.
+"""
+
+import weakref
+import math
+import numpy as np
+import py_trees
+import shapely
+
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import GameTime
+from srunner.scenariomanager.traffic_events import TrafficEvent, TrafficEventType
+
+
+class Criterion(py_trees.behaviour.Behaviour):
+
+ """
+ Base class for all criteria used to evaluate a scenario for success/failure
+
+ Important parameters (PUBLIC):
+ - name: Name of the criterion
+ - expected_value_success: Result in case of success
+ (e.g. max_speed, zero collisions, ...)
+ - expected_value_acceptable: Result that does not mean a failure,
+ but is not good enough for a success
+ - actual_value: Actual result after running the scenario
+ - test_status: Used to access the result of the criterion
+ - optional: Indicates if a criterion is optional (not used for overall analysis)
+ """
+
+ def __init__(self,
+ name,
+ actor,
+ expected_value_success,
+ expected_value_acceptable=None,
+ optional=False,
+ terminate_on_failure=False):
+ super(Criterion, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._terminate_on_failure = terminate_on_failure
+
+ self.name = name
+ self.actor = actor
+ self.test_status = "INIT"
+ self.expected_value_success = expected_value_success
+ self.expected_value_acceptable = expected_value_acceptable
+ self.actual_value = 0
+ self.optional = optional
+ self.list_traffic_events = []
+
+ def initialise(self):
+ """
+ Initialise the criterion. Can be extended by the user-derived class
+ """
+ self.logger.debug("%s.initialise()" % (self.__class__.__name__))
+
+ def terminate(self, new_status):
+ """
+ Terminate the criterion. Can be extended by the user-derived class
+ """
+ if (self.test_status == "RUNNING") or (self.test_status == "INIT"):
+ self.test_status = "SUCCESS"
+
+ self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+
+class MaxVelocityTest(Criterion):
+
+ """
+ This class contains an atomic test for maximum velocity.
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - max_velocity_allowed: maximum allowed velocity in m/s
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+
+ def __init__(self, actor, max_velocity_allowed, optional=False, name="CheckMaximumVelocity"):
+ """
+ Setup actor and maximum allowed velovity
+ """
+ super(MaxVelocityTest, self).__init__(name, actor, max_velocity_allowed, None, optional)
+
+ def update(self):
+ """
+ Check velocity
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self.actor is None:
+ return new_status
+
+ velocity = CarlaDataProvider.get_velocity(self.actor)
+
+ self.actual_value = max(velocity, self.actual_value)
+
+ if velocity > self.expected_value_success:
+ self.test_status = "FAILURE"
+ else:
+ self.test_status = "SUCCESS"
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class DrivenDistanceTest(Criterion):
+
+ """
+ This class contains an atomic test to check the driven distance
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - distance_success: If the actor's driven distance is more than this value (in meters),
+ the test result is SUCCESS
+ - distance_acceptable: If the actor's driven distance is more than this value (in meters),
+ the test result is ACCEPTABLE
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+
+ def __init__(self,
+ actor,
+ distance_success,
+ distance_acceptable=None,
+ optional=False,
+ name="CheckDrivenDistance"):
+ """
+ Setup actor
+ """
+ super(DrivenDistanceTest, self).__init__(name, actor, distance_success, distance_acceptable, optional)
+ self._last_location = None
+
+ def initialise(self):
+ self._last_location = CarlaDataProvider.get_location(self.actor)
+ super(DrivenDistanceTest, self).initialise()
+
+ def update(self):
+ """
+ Check distance
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self.actor is None:
+ return new_status
+
+ location = CarlaDataProvider.get_location(self.actor)
+
+ if location is None:
+ return new_status
+
+ if self._last_location is None:
+ self._last_location = location
+ return new_status
+
+ self.actual_value += location.distance(self._last_location)
+ self._last_location = location
+
+ if self.actual_value > self.expected_value_success:
+ self.test_status = "SUCCESS"
+ elif (self.expected_value_acceptable is not None and
+ self.actual_value > self.expected_value_acceptable):
+ self.test_status = "ACCEPTABLE"
+ else:
+ self.test_status = "RUNNING"
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ Set final status
+ """
+ if self.test_status != "SUCCESS":
+ self.test_status = "FAILURE"
+ self.actual_value = round(self.actual_value, 2)
+ super(DrivenDistanceTest, self).terminate(new_status)
+
+
+class AverageVelocityTest(Criterion):
+
+ """
+ This class contains an atomic test for average velocity.
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - avg_velocity_success: If the actor's average velocity is more than this value (in m/s),
+ the test result is SUCCESS
+ - avg_velocity_acceptable: If the actor's average velocity is more than this value (in m/s),
+ the test result is ACCEPTABLE
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+
+ def __init__(self,
+ actor,
+ avg_velocity_success,
+ avg_velocity_acceptable=None,
+ optional=False,
+ name="CheckAverageVelocity"):
+ """
+ Setup actor and average velovity expected
+ """
+ super(AverageVelocityTest, self).__init__(name, actor,
+ avg_velocity_success,
+ avg_velocity_acceptable,
+ optional)
+ self._last_location = None
+ self._distance = 0.0
+
+ def initialise(self):
+ self._last_location = CarlaDataProvider.get_location(self.actor)
+ super(AverageVelocityTest, self).initialise()
+
+ def update(self):
+ """
+ Check velocity
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self.actor is None:
+ return new_status
+
+ location = CarlaDataProvider.get_location(self.actor)
+
+ if location is None:
+ return new_status
+
+ if self._last_location is None:
+ self._last_location = location
+ return new_status
+
+ self._distance += location.distance(self._last_location)
+ self._last_location = location
+
+ elapsed_time = GameTime.get_time()
+ if elapsed_time > 0.0:
+ self.actual_value = self._distance / elapsed_time
+
+ if self.actual_value > self.expected_value_success:
+ self.test_status = "SUCCESS"
+ elif (self.expected_value_acceptable is not None and
+ self.actual_value > self.expected_value_acceptable):
+ self.test_status = "ACCEPTABLE"
+ else:
+ self.test_status = "RUNNING"
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ Set final status
+ """
+ if self.test_status == "RUNNING":
+ self.test_status = "FAILURE"
+ super(AverageVelocityTest, self).terminate(new_status)
+
+
+class CollisionTest(Criterion):
+
+ """
+ This class contains an atomic test for collisions.
+
+ Args:
+ - actor (carla.Actor): CARLA actor to be used for this test
+ - other_actor (carla.Actor): only collisions with this actor will be registered
+ - other_actor_type (str): only collisions with actors including this type_id will count.
+ Additionally, the "miscellaneous" tag can also be used to include all static objects in the scene
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+
+ MIN_AREA_OF_COLLISION = 3 # If closer than this distance, the collision is ignored
+ MAX_AREA_OF_COLLISION = 5 # If further than this distance, the area is forgotten
+ MAX_ID_TIME = 5 # Amount of time the last collision if is remembered
+
+ def __init__(self, actor, other_actor=None, other_actor_type=None,
+ optional=False, name="CollisionTest", terminate_on_failure=False):
+ """
+ Construction with sensor setup
+ """
+ super(CollisionTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ world = self.actor.get_world()
+ blueprint = world.get_blueprint_library().find('sensor.other.collision')
+ self._collision_sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self.actor)
+ self._collision_sensor.listen(lambda event: self._count_collisions(weakref.ref(self), event))
+
+ self.other_actor = other_actor
+ self.other_actor_type = other_actor_type
+ self.registered_collisions = []
+ self.last_id = None
+ self.collision_time = None
+
+ def update(self):
+ """
+ Check collision count
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ actor_location = CarlaDataProvider.get_location(self.actor)
+ new_registered_collisions = []
+
+ # Loops through all the previous registered collisions
+ for collision_location in self.registered_collisions:
+
+ # Get the distance to the collision point
+ distance_vector = actor_location - collision_location
+ distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2))
+
+ # If far away from a previous collision, forget it
+ if distance <= self.MAX_AREA_OF_COLLISION:
+ new_registered_collisions.append(collision_location)
+
+ self.registered_collisions = new_registered_collisions
+
+ if self.last_id and GameTime.get_time() - self.collision_time > self.MAX_ID_TIME:
+ self.last_id = None
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ Cleanup sensor
+ """
+ if self._collision_sensor is not None:
+ self._collision_sensor.destroy()
+ self._collision_sensor = None
+
+ super(CollisionTest, self).terminate(new_status)
+
+ @staticmethod
+ def _count_collisions(weak_self, event): # pylint: disable=too-many-return-statements
+ """
+ Callback to update collision count
+ """
+ self = weak_self()
+ if not self:
+ return
+
+ actor_location = CarlaDataProvider.get_location(self.actor)
+
+ # Ignore the current one if it is the same id as before
+ if self.last_id == event.other_actor.id:
+ return
+
+ # Filter to only a specific actor
+ if self.other_actor and self.other_actor.id != event.other_actor.id:
+ return
+
+ # Filter to only a specific type
+ if self.other_actor_type:
+ if self.other_actor_type == "miscellaneous":
+ if "traffic" not in event.other_actor.type_id \
+ and "static" not in event.other_actor.type_id:
+ return
+ else:
+ if self.other_actor_type not in event.other_actor.type_id:
+ return
+
+ # Ignore it if its too close to a previous collision (avoid micro collisions)
+ for collision_location in self.registered_collisions:
+
+ distance_vector = actor_location - collision_location
+ distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2))
+
+ if distance <= self.MIN_AREA_OF_COLLISION:
+ return
+
+ if ('static' in event.other_actor.type_id or 'traffic' in event.other_actor.type_id) \
+ and 'sidewalk' not in event.other_actor.type_id:
+ actor_type = TrafficEventType.COLLISION_STATIC
+ elif 'vehicle' in event.other_actor.type_id:
+ actor_type = TrafficEventType.COLLISION_VEHICLE
+ elif 'walker' in event.other_actor.type_id:
+ actor_type = TrafficEventType.COLLISION_PEDESTRIAN
+ else:
+ return
+
+ collision_event = TrafficEvent(event_type=actor_type)
+ collision_event.set_dict({
+ 'type': event.other_actor.type_id,
+ 'id': event.other_actor.id,
+ 'x': actor_location.x,
+ 'y': actor_location.y,
+ 'z': actor_location.z})
+ collision_event.set_message(
+ "Agent collided against object with type={} and id={} at (x={}, y={}, z={})".format(
+ event.other_actor.type_id,
+ event.other_actor.id,
+ round(actor_location.x, 3),
+ round(actor_location.y, 3),
+ round(actor_location.z, 3)))
+
+ self.test_status = "FAILURE"
+ self.actual_value += 1
+ self.collision_time = GameTime.get_time()
+
+ self.registered_collisions.append(actor_location)
+ self.list_traffic_events.append(collision_event)
+
+ # Number 0: static objects -> ignore it
+ if event.other_actor.id != 0:
+ self.last_id = event.other_actor.id
+
+
+class ActorSpeedAboveThresholdTest(Criterion):
+
+ """
+ This test will fail if the actor has had its linear velocity lower than a specific value for
+ a specific amount of time
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - speed_threshold: speed required
+ - below_threshold_max_time: Maximum time (in seconds) the actor can remain under the speed threshold
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ """
+
+ def __init__(self, actor, speed_threshold, below_threshold_max_time,
+ name="ActorSpeedAboveThresholdTest", terminate_on_failure=False):
+ """
+ Class constructor.
+ """
+ super(ActorSpeedAboveThresholdTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._speed_threshold = speed_threshold
+ self._below_threshold_max_time = below_threshold_max_time
+ self._time_last_valid_state = None
+
+ def update(self):
+ """
+ Check if the actor speed is above the speed_threshold
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ linear_speed = CarlaDataProvider.get_velocity(self._actor)
+ if linear_speed is not None:
+ if linear_speed < self._speed_threshold and self._time_last_valid_state:
+ if (GameTime.get_time() - self._time_last_valid_state) > self._below_threshold_max_time:
+ # Game over. The actor has been "blocked" for too long
+ self.test_status = "FAILURE"
+
+ # record event
+ vehicle_location = CarlaDataProvider.get_location(self._actor)
+ blocked_event = TrafficEvent(event_type=TrafficEventType.VEHICLE_BLOCKED)
+ ActorSpeedAboveThresholdTest._set_event_message(blocked_event, vehicle_location)
+ ActorSpeedAboveThresholdTest._set_event_dict(blocked_event, vehicle_location)
+ self.list_traffic_events.append(blocked_event)
+ else:
+ self._time_last_valid_state = GameTime.get_time()
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ @staticmethod
+ def _set_event_message(event, location):
+ """
+ Sets the message of the event
+ """
+
+ event.set_message('Agent got blocked at (x={}, y={}, z={})'.format(round(location.x, 3),
+ round(location.y, 3),
+ round(location.z, 3)))
+
+ @staticmethod
+ def _set_event_dict(event, location):
+ """
+ Sets the dictionary of the event
+ """
+ event.set_dict({
+ 'x': location.x,
+ 'y': location.y,
+ 'z': location.z,
+ })
+
+
+class KeepLaneTest(Criterion):
+
+ """
+ This class contains an atomic test for keeping lane.
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+
+ def __init__(self, actor, optional=False, name="CheckKeepLane"):
+ """
+ Construction with sensor setup
+ """
+ super(KeepLaneTest, self).__init__(name, actor, 0, None, optional)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ world = self.actor.get_world()
+ blueprint = world.get_blueprint_library().find('sensor.other.lane_invasion')
+ self._lane_sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self.actor)
+ self._lane_sensor.listen(lambda event: self._count_lane_invasion(weakref.ref(self), event))
+
+ def update(self):
+ """
+ Check lane invasion count
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self.actual_value > 0:
+ self.test_status = "FAILURE"
+ else:
+ self.test_status = "SUCCESS"
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ Cleanup sensor
+ """
+ if self._lane_sensor is not None:
+ self._lane_sensor.destroy()
+ self._lane_sensor = None
+ super(KeepLaneTest, self).terminate(new_status)
+
+ @staticmethod
+ def _count_lane_invasion(weak_self, event):
+ """
+ Callback to update lane invasion count
+ """
+ self = weak_self()
+ if not self:
+ return
+ self.actual_value += 1
+
+
+class ReachedRegionTest(Criterion):
+
+ """
+ This class contains the reached region test
+ The test is a success if the actor reaches a specified region
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - min_x, max_x, min_y, max_y: Bounding box of the checked region
+ """
+
+ def __init__(self, actor, min_x, max_x, min_y, max_y, name="ReachedRegionTest"):
+ """
+ Setup trigger region (rectangle provided by
+ [min_x,min_y] and [max_x,max_y]
+ """
+ super(ReachedRegionTest, self).__init__(name, actor, 0)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._min_x = min_x
+ self._max_x = max_x
+ self._min_y = min_y
+ self._max_y = max_y
+
+ def update(self):
+ """
+ Check if the actor location is within trigger region
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ in_region = False
+ if self.test_status != "SUCCESS":
+ in_region = (location.x > self._min_x and location.x < self._max_x) and (
+ location.y > self._min_y and location.y < self._max_y)
+ if in_region:
+ self.test_status = "SUCCESS"
+ else:
+ self.test_status = "RUNNING"
+
+ if self.test_status == "SUCCESS":
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class OffRoadTest(Criterion):
+
+ """
+ Atomic containing a test to detect when an actor deviates from the driving lanes. This atomic can
+ fail when actor has spent a specific time outside driving lanes (defined by OpenDRIVE). Simplified
+ version of OnSidewalkTest, and doesn't relly on waypoints with *Sidewalk* lane types
+
+ Args:
+ actor (carla.Actor): CARLA actor to be used for this test
+ duration (float): Time spent at sidewalks before the atomic fails.
+ If terminate_on_failure isn't active, this is ignored.
+ optional (bool): If True, the result is not considered for an overall pass/fail result
+ when using the output argument
+ terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met.
+ """
+
+ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="OffRoadTest"):
+ """
+ Setup of the variables
+ """
+ super(OffRoadTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ self._map = CarlaDataProvider.get_map()
+ self._offroad = False
+
+ self._duration = duration
+ self._prev_time = None
+ self._time_offroad = 0
+
+ def update(self):
+ """
+ First, transforms the actor's current position to its corresponding waypoint. This is
+ filtered to only use waypoints of type Driving or Parking. Depending on these results,
+ the actor will be considered to be outside (or inside) driving lanes.
+
+ returns:
+ py_trees.common.Status.FAILURE: when the actor has spent a given duration outside driving lanes
+ py_trees.common.Status.RUNNING: the rest of the time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ current_location = CarlaDataProvider.get_location(self.actor)
+
+ # Get the waypoint at the current location to see if the actor is offroad
+ drive_waypoint = self._map.get_waypoint(
+ current_location,
+ project_to_road=False
+ )
+ park_waypoint = self._map.get_waypoint(
+ current_location,
+ project_to_road=False,
+ lane_type=carla.LaneType.Parking
+ )
+ if drive_waypoint or park_waypoint:
+ self._offroad = False
+ else:
+ self._offroad = True
+
+ # Counts the time offroad
+ if self._offroad:
+ if self._prev_time is None:
+ self._prev_time = GameTime.get_time()
+ else:
+ curr_time = GameTime.get_time()
+ self._time_offroad += curr_time - self._prev_time
+ self._prev_time = curr_time
+ else:
+ self._prev_time = None
+
+ if self._time_offroad > self._duration:
+ self.test_status = "FAILURE"
+
+ if self._terminate_on_failure and self.test_status == "FAILURE":
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class EndofRoadTest(Criterion):
+
+ """
+ Atomic containing a test to detect when an actor has changed to a different road
+
+ Args:
+ actor (carla.Actor): CARLA actor to be used for this test
+ duration (float): Time spent after ending the road before the atomic fails.
+ If terminate_on_failure isn't active, this is ignored.
+ optional (bool): If True, the result is not considered for an overall pass/fail result
+ when using the output argument
+ terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met.
+ """
+
+ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="EndofRoadTest"):
+ """
+ Setup of the variables
+ """
+ super(EndofRoadTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ self._map = CarlaDataProvider.get_map()
+ self._end_of_road = False
+
+ self._duration = duration
+ self._start_time = None
+ self._time_end_road = 0
+ self._road_id = None
+
+ def update(self):
+ """
+ First, transforms the actor's current position to its corresponding waypoint. Then the road id
+ is compared with the initial one and if that's the case, a time is started
+
+ returns:
+ py_trees.common.Status.FAILURE: when the actor has spent a given duration outside driving lanes
+ py_trees.common.Status.RUNNING: the rest of the time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ current_location = CarlaDataProvider.get_location(self.actor)
+ current_waypoint = self._map.get_waypoint(current_location)
+
+ # Get the current road id
+ if self._road_id is None:
+ self._road_id = current_waypoint.road_id
+
+ else:
+ # Wait until the actor has left the road
+ if self._road_id != current_waypoint.road_id or self._start_time:
+
+ # Start counting
+ if self._start_time is None:
+ self._start_time = GameTime.get_time()
+ return new_status
+
+ curr_time = GameTime.get_time()
+ self._time_end_road = curr_time - self._start_time
+
+ if self._time_end_road > self._duration:
+ self.test_status = "FAILURE"
+ self.actual_value += 1
+ return py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class OnSidewalkTest(Criterion):
+
+ """
+ Atomic containing a test to detect sidewalk invasions of a specific actor. This atomic can
+ fail when actor has spent a specific time outside driving lanes (defined by OpenDRIVE).
+
+ Args:
+ actor (carla.Actor): CARLA actor to be used for this test
+ duration (float): Time spent at sidewalks before the atomic fails.
+ If terminate_on_failure isn't active, this is ignored.
+ optional (bool): If True, the result is not considered for an overall pass/fail result
+ when using the output argument
+ terminate_on_failure (bool): If True, the atomic will fail when the duration condition has been met.
+ """
+
+ def __init__(self, actor, duration=0, optional=False, terminate_on_failure=False, name="OnSidewalkTest"):
+ """
+ Construction with sensor setup
+ """
+ super(OnSidewalkTest, self).__init__(name, actor, 0, None, optional, terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ self._actor = actor
+ self._map = CarlaDataProvider.get_map()
+ self._onsidewalk_active = False
+ self._outside_lane_active = False
+
+ self._actor_location = self._actor.get_location()
+ self._wrong_sidewalk_distance = 0
+ self._wrong_outside_lane_distance = 0
+ self._sidewalk_start_location = None
+ self._outside_lane_start_location = None
+ self._duration = duration
+ self._prev_time = None
+ self._time_outside_lanes = 0
+
+ def update(self):
+ """
+ First, transforms the actor's current position as well as its four corners to their
+ corresponding waypoints. Depending on their lane type, the actor will be considered to be
+ outside (or inside) driving lanes.
+
+ returns:
+ py_trees.common.Status.FAILURE: when the actor has spent a given duration outside
+ driving lanes and terminate_on_failure is active
+ py_trees.common.Status.RUNNING: the rest of the time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._terminate_on_failure and self.test_status == "FAILURE":
+ new_status = py_trees.common.Status.FAILURE
+
+ # Some of the vehicle parameters
+ current_tra = CarlaDataProvider.get_transform(self._actor)
+ current_loc = current_tra.location
+ current_wp = self._map.get_waypoint(current_loc, lane_type=carla.LaneType.Any)
+
+ # Case 1) Car center is at a sidewalk
+ if current_wp.lane_type == carla.LaneType.Sidewalk:
+ if not self._onsidewalk_active:
+ self._onsidewalk_active = True
+ self._sidewalk_start_location = current_loc
+
+ # Case 2) Not inside allowed zones (Driving and Parking)
+ elif current_wp.lane_type != carla.LaneType.Driving \
+ and current_wp.lane_type != carla.LaneType.Parking:
+
+ # Get the vertices of the vehicle
+ heading_vec = current_tra.get_forward_vector()
+ heading_vec.z = 0
+ heading_vec = heading_vec / math.sqrt(math.pow(heading_vec.x, 2) + math.pow(heading_vec.y, 2))
+ perpendicular_vec = carla.Vector3D(-heading_vec.y, heading_vec.x, 0)
+
+ extent = self.actor.bounding_box.extent
+ x_boundary_vector = heading_vec * extent.x
+ y_boundary_vector = perpendicular_vec * extent.y
+
+ bbox = [
+ current_loc + carla.Location(x_boundary_vector - y_boundary_vector),
+ current_loc + carla.Location(x_boundary_vector + y_boundary_vector),
+ current_loc + carla.Location(-1 * x_boundary_vector - y_boundary_vector),
+ current_loc + carla.Location(-1 * x_boundary_vector + y_boundary_vector)]
+
+ bbox_wp = [
+ self._map.get_waypoint(bbox[0], lane_type=carla.LaneType.Any),
+ self._map.get_waypoint(bbox[1], lane_type=carla.LaneType.Any),
+ self._map.get_waypoint(bbox[2], lane_type=carla.LaneType.Any),
+ self._map.get_waypoint(bbox[3], lane_type=carla.LaneType.Any)]
+
+ # Case 2.1) Not quite outside yet
+ if bbox_wp[0].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
+ or bbox_wp[1].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
+ or bbox_wp[2].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking) \
+ or bbox_wp[3].lane_type == (carla.LaneType.Driving or carla.LaneType.Parking):
+
+ self._onsidewalk_active = False
+ self._outside_lane_active = False
+
+ # Case 2.2) At the mini Shoulders between Driving and Sidewalk
+ elif bbox_wp[0].lane_type == carla.LaneType.Sidewalk \
+ or bbox_wp[1].lane_type == carla.LaneType.Sidewalk \
+ or bbox_wp[2].lane_type == carla.LaneType.Sidewalk \
+ or bbox_wp[3].lane_type == carla.LaneType.Sidewalk:
+
+ if not self._onsidewalk_active:
+ self._onsidewalk_active = True
+ self._sidewalk_start_location = current_loc
+
+ else:
+ distance_vehicle_wp = current_loc.distance(current_wp.transform.location)
+
+ # Case 2.3) Outside lane
+ if distance_vehicle_wp >= current_wp.lane_width / 2:
+
+ if not self._outside_lane_active:
+ self._outside_lane_active = True
+ self._outside_lane_start_location = current_loc
+
+ # Case 2.4) Very very edge case (but still inside driving lanes)
+ else:
+ self._onsidewalk_active = False
+ self._outside_lane_active = False
+
+ # Case 3) Driving and Parking conditions
+ else:
+ # Check for false positives at junctions
+ if current_wp.is_junction:
+ distance_vehicle_wp = math.sqrt(
+ math.pow(current_wp.transform.location.x - current_loc.x, 2) +
+ math.pow(current_wp.transform.location.y - current_loc.y, 2))
+
+ if distance_vehicle_wp <= current_wp.lane_width / 2:
+ self._onsidewalk_active = False
+ self._outside_lane_active = False
+ # Else, do nothing, the waypoint is too far to consider it a correct position
+ else:
+
+ self._onsidewalk_active = False
+ self._outside_lane_active = False
+
+ # Counts the time offroad
+ if self._onsidewalk_active or self._outside_lane_active:
+ if self._prev_time is None:
+ self._prev_time = GameTime.get_time()
+ else:
+ curr_time = GameTime.get_time()
+ self._time_outside_lanes += curr_time - self._prev_time
+ self._prev_time = curr_time
+ else:
+ self._prev_time = None
+
+ if self._time_outside_lanes > self._duration:
+ self.test_status = "FAILURE"
+
+ # Update the distances
+ distance_vector = CarlaDataProvider.get_location(self._actor) - self._actor_location
+ distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2))
+
+ if distance >= 0.02: # Used to avoid micro-changes adding to considerable sums
+ self._actor_location = CarlaDataProvider.get_location(self._actor)
+
+ if self._onsidewalk_active:
+ self._wrong_sidewalk_distance += distance
+ elif self._outside_lane_active:
+ # Only add if car is outside the lane but ISN'T in a junction
+ self._wrong_outside_lane_distance += distance
+
+ # Register the sidewalk event
+ if not self._onsidewalk_active and self._wrong_sidewalk_distance > 0:
+
+ self.actual_value += 1
+
+ onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION)
+ self._set_event_message(
+ onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance)
+ self._set_event_dict(
+ onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance)
+
+ self._onsidewalk_active = False
+ self._wrong_sidewalk_distance = 0
+ self.list_traffic_events.append(onsidewalk_event)
+
+ # Register the outside of a lane event
+ if not self._outside_lane_active and self._wrong_outside_lane_distance > 0:
+
+ self.actual_value += 1
+
+ outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION)
+ self._set_event_message(
+ outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance)
+ self._set_event_dict(
+ outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance)
+
+ self._outside_lane_active = False
+ self._wrong_outside_lane_distance = 0
+ self.list_traffic_events.append(outsidelane_event)
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ If there is currently an event running, it is registered
+ """
+ # If currently at a sidewalk, register the event
+ if self._onsidewalk_active:
+
+ self.actual_value += 1
+
+ onsidewalk_event = TrafficEvent(event_type=TrafficEventType.ON_SIDEWALK_INFRACTION)
+ self._set_event_message(
+ onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance)
+ self._set_event_dict(
+ onsidewalk_event, self._sidewalk_start_location, self._wrong_sidewalk_distance)
+
+ self._onsidewalk_active = False
+ self._wrong_sidewalk_distance = 0
+ self.list_traffic_events.append(onsidewalk_event)
+
+ # If currently outside of our lane, register the event
+ if self._outside_lane_active:
+
+ self.actual_value += 1
+
+ outsidelane_event = TrafficEvent(event_type=TrafficEventType.OUTSIDE_LANE_INFRACTION)
+ self._set_event_message(
+ outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance)
+ self._set_event_dict(
+ outsidelane_event, self._outside_lane_start_location, self._wrong_outside_lane_distance)
+
+ self._outside_lane_active = False
+ self._wrong_outside_lane_distance = 0
+ self.list_traffic_events.append(outsidelane_event)
+
+ super(OnSidewalkTest, self).terminate(new_status)
+
+ def _set_event_message(self, event, location, distance):
+ """
+ Sets the message of the event
+ """
+ if event.get_type() == TrafficEventType.ON_SIDEWALK_INFRACTION:
+ message_start = 'Agent invaded the sidewalk'
+ else:
+ message_start = 'Agent went outside the lane'
+
+ event.set_message(
+ '{} for about {} meters, starting at (x={}, y={}, z={})'.format(
+ message_start,
+ round(distance, 3),
+ round(location.x, 3),
+ round(location.y, 3),
+ round(location.z, 3)))
+
+ def _set_event_dict(self, event, location, distance):
+ """
+ Sets the dictionary of the event
+ """
+ event.set_dict({
+ 'x': location.x,
+ 'y': location.y,
+ 'z': location.z,
+ 'distance': distance})
+
+
+class OutsideRouteLanesTest(Criterion):
+
+ """
+ Atomic to detect if the vehicle is either on a sidewalk or at a wrong lane. The distance spent outside
+ is computed and it is returned as a percentage of the route distance traveled.
+
+ Args:
+ actor (carla.ACtor): CARLA actor to be used for this test
+ route (list [carla.Location, connection]): series of locations representing the route waypoints
+ optional (bool): If True, the result is not considered for an overall pass/fail result
+ """
+
+ ALLOWED_OUT_DISTANCE = 1.3 # At least 0.5, due to the mini-shoulder between lanes and sidewalks
+ MAX_ALLOWED_VEHICLE_ANGLE = 120.0 # Maximum angle between the yaw and waypoint lane
+ MAX_ALLOWED_WAYPOINT_ANGLE = 150.0 # Maximum change between the yaw-lane angle between frames
+ WINDOWS_SIZE = 3 # Amount of additional waypoints checked (in case the first on fails)
+
+ def __init__(self, actor, route, optional=False, name="OutsideRouteLanesTest"):
+ """
+ Constructor
+ """
+ super(OutsideRouteLanesTest, self).__init__(name, actor, 0, None, optional)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ self._actor = actor
+ self._route = route
+ self._current_index = 0
+ self._route_length = len(self._route)
+ self._waypoints, _ = zip(*self._route)
+
+ self._map = CarlaDataProvider.get_map()
+ self._pre_ego_waypoint = self._map.get_waypoint(self._actor.get_location())
+
+ self._outside_lane_active = False
+ self._wrong_lane_active = False
+ self._last_road_id = None
+ self._last_lane_id = None
+ self._total_distance = 0
+ self._wrong_distance = 0
+
+ def update(self):
+ """
+ Transforms the actor location and its four corners to waypoints. Depending on its types,
+ the actor will be considered to be at driving lanes, sidewalk or offroad.
+
+ returns:
+ py_trees.common.Status.FAILURE: when the actor has left driving and terminate_on_failure is active
+ py_trees.common.Status.RUNNING: the rest of the time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ # Some of the vehicle parameters
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ # 1) Check if outside route lanes
+ self._is_outside_driving_lanes(location)
+ self._is_at_wrong_lane(location)
+
+ if self._outside_lane_active or self._wrong_lane_active:
+ self.test_status = "FAILURE"
+
+ # 2) Get the traveled distance
+ for index in range(self._current_index + 1,
+ min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)):
+ # Get the dot product to know if it has passed this location
+ index_location = self._waypoints[index]
+ index_waypoint = self._map.get_waypoint(index_location)
+
+ wp_dir = index_waypoint.transform.get_forward_vector() # Waypoint's forward vector
+ wp_veh = location - index_location # vector waypoint - vehicle
+ dot_ve_wp = wp_veh.x * wp_dir.x + wp_veh.y * wp_dir.y + wp_veh.z * wp_dir.z
+
+ if dot_ve_wp > 0:
+ # Get the distance traveled
+ index_location = self._waypoints[index]
+ current_index_location = self._waypoints[self._current_index]
+ new_dist = current_index_location.distance(index_location)
+
+ # Add it to the total distance
+ self._current_index = index
+ self._total_distance += new_dist
+
+ # And to the wrong one if outside route lanes
+ if self._outside_lane_active or self._wrong_lane_active:
+ self._wrong_distance += new_dist
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def _is_outside_driving_lanes(self, location):
+ """
+ Detects if the ego_vehicle is outside driving lanes
+ """
+
+ current_driving_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving, project_to_road=True)
+ current_parking_wp = self._map.get_waypoint(location, lane_type=carla.LaneType.Parking, project_to_road=True)
+
+ driving_distance = location.distance(current_driving_wp.transform.location)
+ if current_parking_wp is not None: # Some towns have no parking
+ parking_distance = location.distance(current_parking_wp.transform.location)
+ else:
+ parking_distance = float('inf')
+
+ if driving_distance >= parking_distance:
+ distance = parking_distance
+ lane_width = current_parking_wp.lane_width
+ else:
+ distance = driving_distance
+ lane_width = current_driving_wp.lane_width
+
+ self._outside_lane_active = bool(distance > (lane_width / 2 + self.ALLOWED_OUT_DISTANCE))
+
+ def _is_at_wrong_lane(self, location):
+ """
+ Detects if the ego_vehicle has invaded a wrong lane
+ """
+
+ current_waypoint = self._map.get_waypoint(location, lane_type=carla.LaneType.Driving, project_to_road=True)
+ current_lane_id = current_waypoint.lane_id
+ current_road_id = current_waypoint.road_id
+
+ # Lanes and roads are too chaotic at junctions
+ if current_waypoint.is_junction:
+ self._wrong_lane_active = False
+ elif self._last_road_id != current_road_id or self._last_lane_id != current_lane_id:
+
+ # Route direction can be considered continuous, except after exiting a junction.
+ if self._pre_ego_waypoint.is_junction:
+ yaw_waypt = current_waypoint.transform.rotation.yaw % 360
+ yaw_actor = self._actor.get_transform().rotation.yaw % 360
+
+ vehicle_lane_angle = (yaw_waypt - yaw_actor) % 360
+
+ if vehicle_lane_angle < self.MAX_ALLOWED_VEHICLE_ANGLE \
+ or vehicle_lane_angle > (360 - self.MAX_ALLOWED_VEHICLE_ANGLE):
+ self._wrong_lane_active = False
+ else:
+ self._wrong_lane_active = True
+
+ else:
+ # Check for a big gap in waypoint directions.
+ yaw_pre_wp = self._pre_ego_waypoint.transform.rotation.yaw % 360
+ yaw_cur_wp = current_waypoint.transform.rotation.yaw % 360
+
+ waypoint_angle = (yaw_pre_wp - yaw_cur_wp) % 360
+
+ if waypoint_angle >= self.MAX_ALLOWED_WAYPOINT_ANGLE \
+ and waypoint_angle <= (360 - self.MAX_ALLOWED_WAYPOINT_ANGLE):
+
+ # Is the ego vehicle going back to the lane, or going out? Take the opposite
+ self._wrong_lane_active = not bool(self._wrong_lane_active)
+ else:
+
+ # Changing to a lane with the same direction
+ self._wrong_lane_active = False
+
+ # Remember the last state
+ self._last_lane_id = current_lane_id
+ self._last_road_id = current_road_id
+ self._pre_ego_waypoint = current_waypoint
+
+ def terminate(self, new_status):
+ """
+ If there is currently an event running, it is registered
+ """
+
+ if self._wrong_distance > 0:
+
+ percentage = self._wrong_distance / self._total_distance * 100
+
+ outside_lane = TrafficEvent(event_type=TrafficEventType.OUTSIDE_ROUTE_LANES_INFRACTION)
+ outside_lane.set_message(
+ "Agent went outside its route lanes for about {} meters "
+ "({}% of the completed route)".format(
+ round(self._wrong_distance, 3),
+ round(percentage, 2)))
+
+ outside_lane.set_dict({
+ 'distance': self._wrong_distance,
+ 'percentage': percentage
+ })
+
+ self._wrong_distance = 0
+ self.list_traffic_events.append(outside_lane)
+ self.actual_value = round(percentage, 2)
+
+ super(OutsideRouteLanesTest, self).terminate(new_status)
+
+
+class WrongLaneTest(Criterion):
+
+ """
+ This class contains an atomic test to detect invasions to wrong direction lanes.
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - optional [optional]: If True, the result is not considered for an overall pass/fail result
+ """
+ MAX_ALLOWED_ANGLE = 120.0
+ MAX_ALLOWED_WAYPOINT_ANGLE = 150.0
+
+ def __init__(self, actor, optional=False, name="WrongLaneTest"):
+ """
+ Construction with sensor setup
+ """
+ super(WrongLaneTest, self).__init__(name, actor, 0, None, optional)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ self._actor = actor
+ self._map = CarlaDataProvider.get_map()
+ self._last_lane_id = None
+ self._last_road_id = None
+
+ self._in_lane = True
+ self._wrong_distance = 0
+ self._actor_location = self._actor.get_location()
+ self._previous_lane_waypoint = self._map.get_waypoint(self._actor.get_location())
+ self._wrong_lane_start_location = None
+
+ def update(self):
+ """
+ Check lane invasion count
+ """
+
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ lane_waypoint = self._map.get_waypoint(self._actor.get_location())
+ current_lane_id = lane_waypoint.lane_id
+ current_road_id = lane_waypoint.road_id
+
+ if (self._last_road_id != current_road_id or self._last_lane_id != current_lane_id) \
+ and not lane_waypoint.is_junction:
+ next_waypoint = lane_waypoint.next(2.0)[0]
+
+ if not next_waypoint:
+ return new_status
+
+ # The waypoint route direction can be considered continuous.
+ # Therefore just check for a big gap in waypoint directions.
+ previous_lane_direction = self._previous_lane_waypoint.transform.get_forward_vector()
+ current_lane_direction = lane_waypoint.transform.get_forward_vector()
+
+ p_lane_vector = np.array([previous_lane_direction.x, previous_lane_direction.y])
+ c_lane_vector = np.array([current_lane_direction.x, current_lane_direction.y])
+
+ waypoint_angle = math.degrees(
+ math.acos(np.clip(np.dot(p_lane_vector, c_lane_vector) /
+ (np.linalg.norm(p_lane_vector) * np.linalg.norm(c_lane_vector)), -1.0, 1.0)))
+
+ if waypoint_angle > self.MAX_ALLOWED_WAYPOINT_ANGLE and self._in_lane:
+
+ self.test_status = "FAILURE"
+ self._in_lane = False
+ self.actual_value += 1
+ self._wrong_lane_start_location = self._actor_location
+
+ else:
+ # Reset variables
+ self._in_lane = True
+
+ # Continuity is broken after a junction so check vehicle-lane angle instead
+ if self._previous_lane_waypoint.is_junction:
+
+ vector_wp = np.array([next_waypoint.transform.location.x - lane_waypoint.transform.location.x,
+ next_waypoint.transform.location.y - lane_waypoint.transform.location.y])
+
+ vector_actor = np.array([math.cos(math.radians(self._actor.get_transform().rotation.yaw)),
+ math.sin(math.radians(self._actor.get_transform().rotation.yaw))])
+
+ vehicle_lane_angle = math.degrees(
+ math.acos(np.clip(np.dot(vector_actor, vector_wp) / (np.linalg.norm(vector_wp)), -1.0, 1.0)))
+
+ if vehicle_lane_angle > self.MAX_ALLOWED_ANGLE:
+
+ self.test_status = "FAILURE"
+ self._in_lane = False
+ self.actual_value += 1
+ self._wrong_lane_start_location = self._actor.get_location()
+
+ # Keep adding "meters" to the counter
+ distance_vector = self._actor.get_location() - self._actor_location
+ distance = math.sqrt(math.pow(distance_vector.x, 2) + math.pow(distance_vector.y, 2))
+
+ if distance >= 0.02: # Used to avoid micro-changes adding add to considerable sums
+ self._actor_location = CarlaDataProvider.get_location(self._actor)
+
+ if not self._in_lane and not lane_waypoint.is_junction:
+ self._wrong_distance += distance
+
+ # Register the event
+ if self._in_lane and self._wrong_distance > 0:
+
+ wrong_way_event = TrafficEvent(event_type=TrafficEventType.WRONG_WAY_INFRACTION)
+ self._set_event_message(wrong_way_event, self._wrong_lane_start_location,
+ self._wrong_distance, current_road_id, current_lane_id)
+ self._set_event_dict(wrong_way_event, self._wrong_lane_start_location,
+ self._wrong_distance, current_road_id, current_lane_id)
+
+ self.list_traffic_events.append(wrong_way_event)
+ self._wrong_distance = 0
+
+ # Remember the last state
+ self._last_lane_id = current_lane_id
+ self._last_road_id = current_road_id
+ self._previous_lane_waypoint = lane_waypoint
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ If there is currently an event running, it is registered
+ """
+ if not self._in_lane:
+
+ lane_waypoint = self._map.get_waypoint(self._actor.get_location())
+ current_lane_id = lane_waypoint.lane_id
+ current_road_id = lane_waypoint.road_id
+
+ wrong_way_event = TrafficEvent(event_type=TrafficEventType.WRONG_WAY_INFRACTION)
+ self._set_event_message(wrong_way_event, self._wrong_lane_start_location,
+ self._wrong_distance, current_road_id, current_lane_id)
+ self._set_event_dict(wrong_way_event, self._wrong_lane_start_location,
+ self._wrong_distance, current_road_id, current_lane_id)
+
+ self._wrong_distance = 0
+ self._in_lane = True
+ self.list_traffic_events.append(wrong_way_event)
+
+ super(WrongLaneTest, self).terminate(new_status)
+
+ def _set_event_message(self, event, location, distance, road_id, lane_id):
+ """
+ Sets the message of the event
+ """
+
+ event.set_message(
+ "Agent invaded a lane in opposite direction for {} meters, starting at (x={}, y={}, z={}). "
+ "road_id={}, lane_id={}".format(
+ round(distance, 3),
+ round(location.x, 3),
+ round(location.y, 3),
+ round(location.z, 3),
+ road_id,
+ lane_id))
+
+ def _set_event_dict(self, event, location, distance, road_id, lane_id):
+ """
+ Sets the dictionary of the event
+ """
+ event.set_dict({
+ 'x': location.x,
+ 'y': location.y,
+ 'z': location.y,
+ 'distance': distance,
+ 'road_id': road_id,
+ 'lane_id': lane_id})
+
+
+class InRadiusRegionTest(Criterion):
+
+ """
+ The test is a success if the actor is within a given radius of a specified region
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - x, y, radius: Position (x,y) and radius (in meters) used to get the checked region
+ """
+
+ def __init__(self, actor, x, y, radius, name="InRadiusRegionTest"):
+ """
+ """
+ super(InRadiusRegionTest, self).__init__(name, actor, 0)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._x = x # pylint: disable=invalid-name
+ self._y = y # pylint: disable=invalid-name
+ self._radius = radius
+
+ def update(self):
+ """
+ Check if the actor location is within trigger region
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ if self.test_status != "SUCCESS":
+ in_radius = math.sqrt(((location.x - self._x)**2) + ((location.y - self._y)**2)) < self._radius
+ if in_radius:
+ route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED)
+ route_completion_event.set_message("Destination was successfully reached")
+ self.list_traffic_events.append(route_completion_event)
+ self.test_status = "SUCCESS"
+ else:
+ self.test_status = "RUNNING"
+
+ if self.test_status == "SUCCESS":
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InRouteTest(Criterion):
+
+ """
+ The test is a success if the actor is never outside route. The actor can go outside of the route
+ but only for a certain amount of distance
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - route: Route to be checked
+ - offroad_max: Maximum distance (in meters) the actor can deviate from the route
+ - offroad_min: Maximum safe distance (in meters). Might eventually cause failure
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ """
+ MAX_ROUTE_PERCENTAGE = 30 # %
+ WINDOWS_SIZE = 5 # Amount of additional waypoints checked
+
+ def __init__(self, actor, route, offroad_min=-1, offroad_max=30, name="InRouteTest", terminate_on_failure=False):
+ """
+ """
+ super(InRouteTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._route = route
+ self._offroad_max = offroad_max
+ # Unless specified, halve of the max value
+ if offroad_min == -1:
+ self._offroad_min = self._offroad_max / 2
+ else:
+ self._offroad_min = self._offroad_min
+
+ self._world = CarlaDataProvider.get_world()
+ self._waypoints, _ = zip(*self._route)
+ self._route_length = len(self._route)
+ self._current_index = 0
+ self._out_route_distance = 0
+ self._in_safe_route = True
+
+ self._accum_meters = []
+ prev_wp = self._waypoints[0]
+ for i, wp in enumerate(self._waypoints):
+ d = wp.distance(prev_wp)
+ if i > 0:
+ accum = self._accum_meters[i - 1]
+ else:
+ accum = 0
+
+ self._accum_meters.append(d + accum)
+ prev_wp = wp
+
+ # Blackboard variable
+ blackv = py_trees.blackboard.Blackboard()
+ _ = blackv.set("InRoute", True)
+
+ def update(self):
+ """
+ Check if the actor location is within trigger region
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ elif self.test_status == "RUNNING" or self.test_status == "INIT":
+
+ off_route = True
+
+ shortest_distance = float('inf')
+ closest_index = -1
+
+ # Get the closest distance
+ for index in range(self._current_index,
+ min(self._current_index + self.WINDOWS_SIZE + 1, self._route_length)):
+ ref_waypoint = self._waypoints[index]
+ distance = math.sqrt(((location.x - ref_waypoint.x) ** 2) + ((location.y - ref_waypoint.y) ** 2))
+ if distance <= shortest_distance:
+ closest_index = index
+ shortest_distance = distance
+
+ if closest_index == -1 or shortest_distance == float('inf'):
+ return new_status
+
+ # Check if the actor is out of route
+ if shortest_distance < self._offroad_max:
+ off_route = False
+ self._in_safe_route = bool(shortest_distance < self._offroad_min)
+
+ # If actor advanced a step, record the distance
+ if self._current_index != closest_index:
+
+ new_dist = self._accum_meters[closest_index] - self._accum_meters[self._current_index]
+
+ # If too far from the route, add it and check if its value
+ if not self._in_safe_route:
+ self._out_route_distance += new_dist
+ out_route_percentage = 100 * self._out_route_distance / self._accum_meters[-1]
+ if out_route_percentage > self.MAX_ROUTE_PERCENTAGE:
+ off_route = True
+
+ self._current_index = closest_index
+
+ if off_route:
+ # Blackboard variable
+ blackv = py_trees.blackboard.Blackboard()
+ _ = blackv.set("InRoute", False)
+
+ route_deviation_event = TrafficEvent(event_type=TrafficEventType.ROUTE_DEVIATION)
+ route_deviation_event.set_message(
+ "Agent deviated from the route at (x={}, y={}, z={})".format(
+ round(location.x, 3),
+ round(location.y, 3),
+ round(location.z, 3)))
+ route_deviation_event.set_dict({
+ 'x': location.x,
+ 'y': location.y,
+ 'z': location.z})
+
+ self.list_traffic_events.append(route_deviation_event)
+
+ self.test_status = "FAILURE"
+ self.actual_value += 1
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class RouteCompletionTest(Criterion):
+
+ """
+ Check at which stage of the route is the actor at each tick
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - route: Route to be checked
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ """
+ DISTANCE_THRESHOLD = 10.0 # meters
+ WINDOWS_SIZE = 2
+
+ def __init__(self, actor, route, name="RouteCompletionTest", terminate_on_failure=False):
+ """
+ """
+ super(RouteCompletionTest, self).__init__(name, actor, 100, terminate_on_failure=terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._route = route
+ self._map = CarlaDataProvider.get_map()
+
+ self._wsize = self.WINDOWS_SIZE
+ self._current_index = 0
+ self._route_length = len(self._route)
+ self._waypoints, _ = zip(*self._route)
+ self.target = self._waypoints[-1]
+
+ self._accum_meters = []
+ prev_wp = self._waypoints[0]
+ for i, wp in enumerate(self._waypoints):
+ d = wp.distance(prev_wp)
+ if i > 0:
+ accum = self._accum_meters[i - 1]
+ else:
+ accum = 0
+
+ self._accum_meters.append(d + accum)
+ prev_wp = wp
+
+ self._traffic_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETION)
+ self.list_traffic_events.append(self._traffic_event)
+ self._percentage_route_completed = 0.0
+
+ def update(self):
+ """
+ Check if the actor location is within trigger region
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ if location is None:
+ return new_status
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ elif self.test_status == "RUNNING" or self.test_status == "INIT":
+
+ for index in range(self._current_index, min(self._current_index + self._wsize + 1, self._route_length)):
+ # Get the dot product to know if it has passed this location
+ ref_waypoint = self._waypoints[index]
+ wp = self._map.get_waypoint(ref_waypoint)
+ wp_dir = wp.transform.get_forward_vector() # Waypoint's forward vector
+ wp_veh = location - ref_waypoint # vector waypoint - vehicle
+ dot_ve_wp = wp_veh.x * wp_dir.x + wp_veh.y * wp_dir.y + wp_veh.z * wp_dir.z
+
+ if dot_ve_wp > 0:
+ # good! segment completed!
+ self._current_index = index
+ self._percentage_route_completed = 100.0 * float(self._accum_meters[self._current_index]) \
+ / float(self._accum_meters[-1])
+ self._traffic_event.set_dict({
+ 'route_completed': self._percentage_route_completed})
+ self._traffic_event.set_message(
+ "Agent has completed > {:.2f}% of the route".format(
+ self._percentage_route_completed))
+
+ if self._percentage_route_completed > 99.0 and location.distance(self.target) < self.DISTANCE_THRESHOLD:
+ route_completion_event = TrafficEvent(event_type=TrafficEventType.ROUTE_COMPLETED)
+ route_completion_event.set_message("Destination was successfully reached")
+ self.list_traffic_events.append(route_completion_event)
+ self.test_status = "SUCCESS"
+ self._percentage_route_completed = 100
+
+ elif self.test_status == "SUCCESS":
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def terminate(self, new_status):
+ """
+ Set test status to failure if not successful and terminate
+ """
+ self.actual_value = round(self._percentage_route_completed, 2)
+
+ if self.test_status == "INIT":
+ self.test_status = "FAILURE"
+ super(RouteCompletionTest, self).terminate(new_status)
+
+
+class RunningRedLightTest(Criterion):
+
+ """
+ Check if an actor is running a red light
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ """
+ DISTANCE_LIGHT = 15 # m
+
+ def __init__(self, actor, name="RunningRedLightTest", terminate_on_failure=False):
+ """
+ Init
+ """
+ super(RunningRedLightTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._world = actor.get_world()
+ self._map = CarlaDataProvider.get_map()
+ self._list_traffic_lights = []
+ self._last_red_light_id = None
+ self.actual_value = 0
+ self.debug = False
+
+ all_actors = self._world.get_actors()
+ for _actor in all_actors:
+ if 'traffic_light' in _actor.type_id:
+ center, waypoints = self.get_traffic_light_waypoints(_actor)
+ self._list_traffic_lights.append((_actor, center, waypoints))
+
+ # pylint: disable=no-self-use
+ def is_vehicle_crossing_line(self, seg1, seg2):
+ """
+ check if vehicle crosses a line segment
+ """
+ line1 = shapely.geometry.LineString([(seg1[0].x, seg1[0].y), (seg1[1].x, seg1[1].y)])
+ line2 = shapely.geometry.LineString([(seg2[0].x, seg2[0].y), (seg2[1].x, seg2[1].y)])
+ inter = line1.intersection(line2)
+
+ return not inter.is_empty
+
+ def update(self):
+ """
+ Check if the actor is running a red light
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ transform = CarlaDataProvider.get_transform(self._actor)
+ location = transform.location
+ if location is None:
+ return new_status
+
+ veh_extent = self._actor.bounding_box.extent.x
+
+ tail_close_pt = self.rotate_point(carla.Vector3D(-0.8 * veh_extent, 0.0, location.z), transform.rotation.yaw)
+ tail_close_pt = location + carla.Location(tail_close_pt)
+
+ tail_far_pt = self.rotate_point(carla.Vector3D(-veh_extent - 1, 0.0, location.z), transform.rotation.yaw)
+ tail_far_pt = location + carla.Location(tail_far_pt)
+
+ for traffic_light, center, waypoints in self._list_traffic_lights:
+
+ if self.debug:
+ z = 2.1
+ if traffic_light.state == carla.TrafficLightState.Red:
+ color = carla.Color(155, 0, 0)
+ elif traffic_light.state == carla.TrafficLightState.Green:
+ color = carla.Color(0, 155, 0)
+ else:
+ color = carla.Color(155, 155, 0)
+ self._world.debug.draw_point(center + carla.Location(z=z), size=0.2, color=color, life_time=0.01)
+ for wp in waypoints:
+ text = "{}.{}".format(wp.road_id, wp.lane_id)
+ self._world.debug.draw_string(
+ wp.transform.location + carla.Location(x=1, z=z), text, color=color, life_time=0.01)
+ self._world.debug.draw_point(
+ wp.transform.location + carla.Location(z=z), size=0.1, color=color, life_time=0.01)
+
+ center_loc = carla.Location(center)
+
+ if self._last_red_light_id and self._last_red_light_id == traffic_light.id:
+ continue
+ if center_loc.distance(location) > self.DISTANCE_LIGHT:
+ continue
+ if traffic_light.state != carla.TrafficLightState.Red:
+ continue
+
+ for wp in waypoints:
+
+ tail_wp = self._map.get_waypoint(tail_far_pt)
+
+ # Calculate the dot product (Might be unscaled, as only its sign is important)
+ ve_dir = CarlaDataProvider.get_transform(self._actor).get_forward_vector()
+ wp_dir = wp.transform.get_forward_vector()
+ dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
+
+ # Check the lane until all the "tail" has passed
+ if tail_wp.road_id == wp.road_id and tail_wp.lane_id == wp.lane_id and dot_ve_wp > 0:
+ # This light is red and is affecting our lane
+ yaw_wp = wp.transform.rotation.yaw
+ lane_width = wp.lane_width
+ location_wp = wp.transform.location
+
+ lft_lane_wp = self.rotate_point(carla.Vector3D(0.4 * lane_width, 0.0, location_wp.z), yaw_wp + 90)
+ lft_lane_wp = location_wp + carla.Location(lft_lane_wp)
+ rgt_lane_wp = self.rotate_point(carla.Vector3D(0.4 * lane_width, 0.0, location_wp.z), yaw_wp - 90)
+ rgt_lane_wp = location_wp + carla.Location(rgt_lane_wp)
+
+ # Is the vehicle traversing the stop line?
+ if self.is_vehicle_crossing_line((tail_close_pt, tail_far_pt), (lft_lane_wp, rgt_lane_wp)):
+
+ self.test_status = "FAILURE"
+ self.actual_value += 1
+ location = traffic_light.get_transform().location
+ red_light_event = TrafficEvent(event_type=TrafficEventType.TRAFFIC_LIGHT_INFRACTION)
+ red_light_event.set_message(
+ "Agent ran a red light {} at (x={}, y={}, z={})".format(
+ traffic_light.id,
+ round(location.x, 3),
+ round(location.y, 3),
+ round(location.z, 3)))
+ red_light_event.set_dict({
+ 'id': traffic_light.id,
+ 'x': location.x,
+ 'y': location.y,
+ 'z': location.z})
+
+ self.list_traffic_events.append(red_light_event)
+ self._last_red_light_id = traffic_light.id
+ break
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+ def rotate_point(self, point, angle):
+ """
+ rotate a given point by a given angle
+ """
+ x_ = math.cos(math.radians(angle)) * point.x - math.sin(math.radians(angle)) * point.y
+ y_ = math.sin(math.radians(angle)) * point.x + math.cos(math.radians(angle)) * point.y
+ return carla.Vector3D(x_, y_, point.z)
+
+ def get_traffic_light_waypoints(self, traffic_light):
+ """
+ get area of a given traffic light
+ """
+ base_transform = traffic_light.get_transform()
+ base_rot = base_transform.rotation.yaw
+ area_loc = base_transform.transform(traffic_light.trigger_volume.location)
+
+ # Discretize the trigger box into points
+ area_ext = traffic_light.trigger_volume.extent
+ x_values = np.arange(-0.9 * area_ext.x, 0.9 * area_ext.x, 1.0) # 0.9 to avoid crossing to adjacent lanes
+
+ area = []
+ for x in x_values:
+ point = self.rotate_point(carla.Vector3D(x, 0, area_ext.z), base_rot)
+ point_location = area_loc + carla.Location(x=point.x, y=point.y)
+ area.append(point_location)
+
+ # Get the waypoints of these points, removing duplicates
+ ini_wps = []
+ for pt in area:
+ wpx = self._map.get_waypoint(pt)
+ # As x_values are arranged in order, only the last one has to be checked
+ if not ini_wps or ini_wps[-1].road_id != wpx.road_id or ini_wps[-1].lane_id != wpx.lane_id:
+ ini_wps.append(wpx)
+
+ # Advance them until the intersection
+ wps = []
+ for wpx in ini_wps:
+ while not wpx.is_intersection:
+ next_wp = wpx.next(0.5)[0]
+ if next_wp and not next_wp.is_intersection:
+ wpx = next_wp
+ else:
+ break
+ wps.append(wpx)
+
+ return area_loc, wps
+
+
+class RunningStopTest(Criterion):
+
+ """
+ Check if an actor is running a stop sign
+
+ Important parameters:
+ - actor: CARLA actor to be used for this test
+ - terminate_on_failure [optional]: If True, the complete scenario will terminate upon failure of this test
+ """
+ PROXIMITY_THRESHOLD = 50.0 # meters
+ SPEED_THRESHOLD = 0.1
+ WAYPOINT_STEP = 1.0 # meters
+
+ def __init__(self, actor, name="RunningStopTest", terminate_on_failure=False):
+ """
+ """
+ super(RunningStopTest, self).__init__(name, actor, 0, terminate_on_failure=terminate_on_failure)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._world = CarlaDataProvider.get_world()
+ self._map = CarlaDataProvider.get_map()
+ self._list_stop_signs = []
+ self._target_stop_sign = None
+ self._stop_completed = False
+ self._affected_by_stop = False
+ self.actual_value = 0
+
+ all_actors = self._world.get_actors()
+ for _actor in all_actors:
+ if 'traffic.stop' in _actor.type_id:
+ self._list_stop_signs.append(_actor)
+
+ @staticmethod
+ def point_inside_boundingbox(point, bb_center, bb_extent):
+ """
+ X
+ :param point:
+ :param bb_center:
+ :param bb_extent:
+ :return:
+ """
+
+ # pylint: disable=invalid-name
+ A = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y - bb_extent.y)
+ B = carla.Vector2D(bb_center.x + bb_extent.x, bb_center.y - bb_extent.y)
+ D = carla.Vector2D(bb_center.x - bb_extent.x, bb_center.y + bb_extent.y)
+ M = carla.Vector2D(point.x, point.y)
+
+ AB = B - A
+ AD = D - A
+ AM = M - A
+ am_ab = AM.x * AB.x + AM.y * AB.y
+ ab_ab = AB.x * AB.x + AB.y * AB.y
+ am_ad = AM.x * AD.x + AM.y * AD.y
+ ad_ad = AD.x * AD.x + AD.y * AD.y
+
+ return am_ab > 0 and am_ab < ab_ab and am_ad > 0 and am_ad < ad_ad
+
+ def is_actor_affected_by_stop(self, actor, stop, multi_step=20):
+ """
+ Check if the given actor is affected by the stop
+ """
+ affected = False
+ # first we run a fast coarse test
+ current_location = actor.get_location()
+ stop_location = stop.get_transform().location
+ if stop_location.distance(current_location) > self.PROXIMITY_THRESHOLD:
+ return affected
+
+ stop_t = stop.get_transform()
+ transformed_tv = stop_t.transform(stop.trigger_volume.location)
+
+ # slower and accurate test based on waypoint's horizon and geometric test
+ list_locations = [current_location]
+ waypoint = self._map.get_waypoint(current_location)
+ for _ in range(multi_step):
+ if waypoint:
+ next_wps = waypoint.next(self.WAYPOINT_STEP)
+ if not next_wps:
+ break
+ waypoint = next_wps[0]
+ if not waypoint:
+ break
+ list_locations.append(waypoint.transform.location)
+
+ for actor_location in list_locations:
+ if self.point_inside_boundingbox(actor_location, transformed_tv, stop.trigger_volume.extent):
+ affected = True
+
+ return affected
+
+ def _scan_for_stop_sign(self):
+ target_stop_sign = None
+
+ ve_tra = CarlaDataProvider.get_transform(self._actor)
+ ve_dir = ve_tra.get_forward_vector()
+
+ wp = self._map.get_waypoint(ve_tra.location)
+ wp_dir = wp.transform.get_forward_vector()
+
+ dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
+
+ if dot_ve_wp > 0: # Ignore all when going in a wrong lane
+ for stop_sign in self._list_stop_signs:
+ if self.is_actor_affected_by_stop(self._actor, stop_sign):
+ # this stop sign is affecting the vehicle
+ target_stop_sign = stop_sign
+ break
+
+ return target_stop_sign
+
+ def update(self):
+ """
+ Check if the actor is running a red light
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = self._actor.get_location()
+ if location is None:
+ return new_status
+
+ if not self._target_stop_sign:
+ # scan for stop signs
+ self._target_stop_sign = self._scan_for_stop_sign()
+ else:
+ # we were in the middle of dealing with a stop sign
+ if not self._stop_completed:
+ # did the ego-vehicle stop?
+ current_speed = CarlaDataProvider.get_velocity(self._actor)
+ if current_speed < self.SPEED_THRESHOLD:
+ self._stop_completed = True
+
+ if not self._affected_by_stop:
+ stop_location = self._target_stop_sign.get_location()
+ stop_extent = self._target_stop_sign.trigger_volume.extent
+
+ if self.point_inside_boundingbox(location, stop_location, stop_extent):
+ self._affected_by_stop = True
+
+ if not self.is_actor_affected_by_stop(self._actor, self._target_stop_sign):
+ # is the vehicle out of the influence of this stop sign now?
+ if not self._stop_completed and self._affected_by_stop:
+ # did we stop?
+ self.actual_value += 1
+ self.test_status = "FAILURE"
+ stop_location = self._target_stop_sign.get_transform().location
+ running_stop_event = TrafficEvent(event_type=TrafficEventType.STOP_INFRACTION)
+ running_stop_event.set_message(
+ "Agent ran a stop with id={} at (x={}, y={}, z={})".format(
+ self._target_stop_sign.id,
+ round(stop_location.x, 3),
+ round(stop_location.y, 3),
+ round(stop_location.z, 3)))
+ running_stop_event.set_dict({
+ 'id': self._target_stop_sign.id,
+ 'x': stop_location.x,
+ 'y': stop_location.y,
+ 'z': stop_location.z})
+
+ self.list_traffic_events.append(running_stop_event)
+
+ # reset state
+ self._target_stop_sign = None
+ self._stop_completed = False
+ self._affected_by_stop = False
+
+ if self._terminate_on_failure and (self.test_status == "FAILURE"):
+ new_status = py_trees.common.Status.FAILURE
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
diff --git a/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py
new file mode 100644
index 0000000..7654aea
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py
@@ -0,0 +1,1233 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides all atomic scenario behaviors that reflect
+trigger conditions to either activate another behavior, or to stop
+another behavior.
+
+For example, such a condition could be "InTriggerRegion", which checks
+that a given actor reached a certain region on the map, and then starts/stops
+a behavior of this actor.
+
+The atomics are implemented with py_trees and make use of the AtomicCondition
+base class
+"""
+
+from __future__ import print_function
+
+import operator
+import datetime
+import math
+import py_trees
+import carla
+
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import calculate_distance
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import GameTime
+from srunner.tools.scenario_helper import get_distance_along_route
+
+import srunner.tools
+
+EPSILON = 0.001
+
+
+class AtomicCondition(py_trees.behaviour.Behaviour):
+
+ """
+ Base class for all atomic conditions used to setup a scenario
+
+ *All behaviors should use this class as parent*
+
+ Important parameters:
+ - name: Name of the atomic condition
+ """
+
+ def __init__(self, name):
+ """
+ Default init. Has to be called via super from derived class
+ """
+ super(AtomicCondition, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self.name = name
+
+ def setup(self, unused_timeout=15):
+ """
+ Default setup
+ """
+ self.logger.debug("%s.setup()" % (self.__class__.__name__))
+ return True
+
+ def initialise(self):
+ """
+ Initialise setup
+ """
+ self.logger.debug("%s.initialise()" % (self.__class__.__name__))
+
+ def terminate(self, new_status):
+ """
+ Default terminate. Can be extended in derived class
+ """
+ self.logger.debug("%s.terminate()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+
+class InTriggerDistanceToOSCPosition(AtomicCondition):
+
+ """
+ OpenSCENARIO atomic
+ This class contains the trigger condition for a distance to an OpenSCENARIO position
+
+ Args:
+ actor (carla.Actor): CARLA actor to execute the behavior
+ osc_position (str): OpenSCENARIO position
+ distance (float): Trigger distance between the actor and the target location in meters
+ name (str): Name of the condition
+
+ The condition terminates with SUCCESS, when the actor reached the target distance to the openSCENARIO position
+ """
+
+ def __init__(self, actor, osc_position, distance, along_route=False,
+ comparison_operator=operator.lt, name="InTriggerDistanceToOSCPosition"):
+ """
+ Setup parameters
+ """
+ super(InTriggerDistanceToOSCPosition, self).__init__(name)
+ self._actor = actor
+ self._osc_position = osc_position
+ self._distance = distance
+ self._along_route = along_route
+ self._comparison_operator = comparison_operator
+ self._map = CarlaDataProvider.get_map()
+
+ if self._along_route:
+ # Get the global route planner, used to calculate the route
+ dao = GlobalRoutePlannerDAO(self._map, 0.5)
+ grp = GlobalRoutePlanner(dao)
+ grp.setup()
+ self._grp = grp
+ else:
+ self._grp = None
+
+ def initialise(self):
+ if self._distance < 0:
+ raise ValueError("distance value must be positive")
+
+ def update(self):
+ """
+ Check if actor is in trigger distance
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ # calculate transform with method in openscenario_parser.py
+ osc_transform = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
+ self._osc_position)
+
+ if osc_transform is not None:
+ osc_location = osc_transform.location
+ actor_location = CarlaDataProvider.get_location(self._actor)
+
+ if self._along_route:
+ # Global planner needs a location at a driving lane
+ actor_location = self._map.get_waypoint(actor_location).transform.location
+ osc_location = self._map.get_waypoint(osc_location).transform.location
+
+ distance = calculate_distance(actor_location, osc_location, self._grp)
+
+ if self._comparison_operator(distance, self._distance):
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class InTimeToArrivalToOSCPosition(AtomicCondition):
+
+ """
+ OpenSCENARIO atomic
+ This class contains a trigger if an actor arrives within a given time to an OpenSCENARIO position
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - osc_position: OpenSCENARIO position
+ - time: The behavior is successful, if TTA is less than _time_ in seconds
+ - name: Name of the condition
+
+ The condition terminates with SUCCESS, when the actor can reach the position within the given time
+ """
+
+ def __init__(self, actor, osc_position, time, along_route=False,
+ comparison_operator=operator.lt, name="InTimeToArrivalToOSCPosition"):
+ """
+ Setup parameters
+ """
+ super(InTimeToArrivalToOSCPosition, self).__init__(name)
+ self._map = CarlaDataProvider.get_map()
+ self._actor = actor
+ self._osc_position = osc_position
+ self._time = float(time)
+ self._along_route = along_route
+ self._comparison_operator = comparison_operator
+
+ if self._along_route:
+ # Get the global route planner, used to calculate the route
+ dao = GlobalRoutePlannerDAO(self._map, 0.5)
+ grp = GlobalRoutePlanner(dao)
+ grp.setup()
+ self._grp = grp
+ else:
+ self._grp = None
+
+ def initialise(self):
+ if self._time < 0:
+ raise ValueError("time value must be positive")
+
+ def update(self):
+ """
+ Check if actor can arrive within trigger time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ # calculate transform with method in openscenario_parser.py
+ try:
+ osc_transform = srunner.tools.openscenario_parser.OpenScenarioParser.convert_position_to_transform(
+ self._osc_position)
+ except AttributeError:
+ return py_trees.common.Status.FAILURE
+ target_location = osc_transform.location
+ actor_location = CarlaDataProvider.get_location(self._actor)
+
+ if target_location is None or actor_location is None:
+ return new_status
+
+ if self._along_route:
+ # Global planner needs a location at a driving lane
+ actor_location = self._map.get_waypoint(actor_location).transform.location
+ target_location = self._map.get_waypoint(target_location).transform.location
+
+ distance = calculate_distance(actor_location, target_location, self._grp)
+
+ actor_velocity = CarlaDataProvider.get_velocity(self._actor)
+
+ # time to arrival
+ if actor_velocity > 0:
+ time_to_arrival = distance / actor_velocity
+ elif distance == 0:
+ time_to_arrival = 0
+ else:
+ time_to_arrival = float('inf')
+
+ if self._comparison_operator(time_to_arrival, self._time):
+ new_status = py_trees.common.Status.SUCCESS
+ return new_status
+
+
+class StandStill(AtomicCondition):
+
+ """
+ This class contains a standstill behavior of a scenario
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - duration: Duration of the behavior in seconds
+
+ The condition terminates with SUCCESS, when the actor does not move
+ """
+
+ def __init__(self, actor, name, duration=float("inf")):
+ """
+ Setup actor
+ """
+ super(StandStill, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+
+ self._duration = duration
+ self._start_time = 0
+
+ def initialise(self):
+ """
+ Initialize the start time of this condition
+ """
+ self._start_time = GameTime.get_time()
+ super(StandStill, self).initialise()
+
+ def update(self):
+ """
+ Check if the _actor stands still (v=0)
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ velocity = CarlaDataProvider.get_velocity(self._actor)
+
+ if velocity > EPSILON:
+ self._start_time = GameTime.get_time()
+
+ if GameTime.get_time() - self._start_time > self._duration:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class RelativeVelocityToOtherActor(AtomicCondition):
+
+ """
+ Atomic containing a comparison between an actor's velocity
+ and another actor's one. The behavior returns SUCCESS when the
+ expected comparison (greater than / less than / equal to) is achieved
+
+ Args:
+ actor (carla.Actor): actor from which the velocity is taken
+ other_actor (carla.Actor): The actor with the reference velocity
+ speed (float): Difference of speed between the actors
+ name (string): Name of the condition
+ comparison_operator: Type "operator", used to compare the two velocities
+ """
+
+ def __init__(self, actor, other_actor, speed, comparison_operator=operator.gt,
+ name="RelativeVelocityToOtherActor"):
+ """
+ Setup the parameters
+ """
+ super(RelativeVelocityToOtherActor, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._other_actor = other_actor
+ self._relative_speed = speed
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Gets the speed of the two actors and compares them according to the comparison operator
+
+ returns:
+ py_trees.common.Status.RUNNING when the comparison fails and
+ py_trees.common.Status.SUCCESS when it succeeds
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ curr_speed = CarlaDataProvider.get_velocity(self._actor)
+ other_speed = CarlaDataProvider.get_velocity(self._other_actor)
+
+ relative_speed = curr_speed - other_speed
+
+ if self._comparison_operator(relative_speed, self._relative_speed):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class TriggerVelocity(AtomicCondition):
+
+ """
+ Atomic containing a comparison between an actor's speed and a reference one.
+ The behavior returns SUCCESS when the expected comparison (greater than /
+ less than / equal to) is achieved.
+
+ Args:
+ actor (carla.Actor): CARLA actor from which the speed will be taken.
+ name (string): Name of the atomic
+ target_velocity (float): velcoity to be compared with the actor's one
+ comparison_operator: Type "operator", used to compare the two velocities
+ """
+
+ def __init__(self, actor, target_velocity, comparison_operator=operator.gt, name="TriggerVelocity"):
+ """
+ Setup the atomic parameters
+ """
+ super(TriggerVelocity, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._target_velocity = target_velocity
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Gets the speed of the actor and compares it with the reference one
+
+ returns:
+ py_trees.common.Status.RUNNING when the comparison fails and
+ py_trees.common.Status.SUCCESS when it succeeds
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ actor_speed = CarlaDataProvider.get_velocity(self._actor)
+
+ if self._comparison_operator(actor_speed, self._target_velocity):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class TriggerAcceleration(AtomicCondition):
+
+ """
+ Atomic containing a comparison between an actor's acceleration
+ and a reference one. The behavior returns SUCCESS when the
+ expected comparison (greater than / less than / equal to) is achieved
+
+ Args:
+ actor (carla.Actor): CARLA actor to execute the behavior
+ name (str): Name of the condition
+ target_acceleration (float): Acceleration reference (in m/s^2) on which the success is dependent
+ comparison_operator (operator): Type "operator", used to compare the two acceleration
+ """
+
+ def __init__(self, actor, target_acceleration, comparison_operator=operator.gt, name="TriggerAcceleration"):
+ """
+ Setup trigger acceleration
+ """
+ super(TriggerAcceleration, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._target_acceleration = target_acceleration
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Gets the accleration of the actor and compares it with the reference one
+
+ returns:
+ py_trees.common.Status.RUNNING when the comparison fails and
+ py_trees.common.Status.SUCCESS when it succeeds
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ acceleration = self._actor.get_acceleration()
+ linear_accel = math.sqrt(math.pow(acceleration.x, 2) +
+ math.pow(acceleration.y, 2) +
+ math.pow(acceleration.z, 2))
+
+ if self._comparison_operator(linear_accel, self._target_acceleration):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class TimeOfDayComparison(AtomicCondition):
+
+ """
+ Atomic containing a comparison between the current time of day of the simulation
+ and a given one. The behavior returns SUCCESS when the
+ expected comparison (greater than / less than / equal to) is achieved
+
+ Args:
+ datetime (datetime): CARLA actor to execute the behavior
+ name (str): Name of the condition
+ target_acceleration (float): Acceleration reference (in m/s^2) on which the success is dependent
+ comparison_operator (operator): Type "operator", used to compare the two acceleration
+ """
+
+ def __init__(self, dattime, comparison_operator=operator.gt, name="TimeOfDayComparison"):
+ """
+ """
+ super(TimeOfDayComparison, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._datetime = datetime.datetime.strptime(dattime, "%Y-%m-%dT%H:%M:%S")
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Gets the time of day of the simulation and compares it with the reference one
+
+ returns:
+ py_trees.common.Status.RUNNING when the comparison fails and
+ py_trees.common.Status.SUCCESS when it succeeds
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ try:
+ check_dtime = operator.attrgetter("Datetime")
+ dtime = check_dtime(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if self._comparison_operator(dtime, self._datetime):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class OSCStartEndCondition(AtomicCondition):
+
+ """
+ This class contains a check if a named story element has started/terminated.
+
+ Important parameters:
+ - element_name: The story element's name attribute
+ - element_type: The element type [act,scene,maneuver,event,action]
+ - rule: Either START or END
+
+ The condition terminates with SUCCESS, when the named story element starts
+ """
+
+ def __init__(self, element_type, element_name, rule, name="OSCStartEndCondition"):
+ """
+ Setup element details
+ """
+ super(OSCStartEndCondition, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._element_type = element_type.upper()
+ self._element_name = element_name
+ self._rule = rule.upper()
+ self._start_time = None
+ self._blackboard = py_trees.blackboard.Blackboard()
+
+ def initialise(self):
+ """
+ Initialize the start time of this condition
+ """
+ self._start_time = GameTime.get_time()
+ super(OSCStartEndCondition, self).initialise()
+
+ def update(self):
+ """
+ Check if the specified story element has started/ended since the beginning of the condition
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ if new_status == py_trees.common.Status.RUNNING:
+ blackboard_variable_name = "({}){}-{}".format(self._element_type, self._element_name, self._rule)
+ element_start_time = self._blackboard.get(blackboard_variable_name)
+ if element_start_time and element_start_time >= self._start_time:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTriggerRegion(AtomicCondition):
+
+ """
+ This class contains the trigger region (condition) of a scenario
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - min_x, max_x, min_y, max_y: bounding box of the trigger region
+
+ The condition terminates with SUCCESS, when the actor reached the target region
+ """
+
+ def __init__(self, actor, min_x, max_x, min_y, max_y, name="TriggerRegion"):
+ """
+ Setup trigger region (rectangle provided by
+ [min_x,min_y] and [max_x,max_y]
+ """
+ super(InTriggerRegion, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._min_x = min_x
+ self._max_x = max_x
+ self._min_y = min_y
+ self._max_y = max_y
+
+ def update(self):
+ """
+ Check if the _actor location is within trigger region
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+
+ if location is None:
+ return new_status
+
+ not_in_region = (location.x < self._min_x or location.x > self._max_x) or (
+ location.y < self._min_y or location.y > self._max_y)
+ if not not_in_region:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTriggerDistanceToVehicle(AtomicCondition):
+
+ """
+ This class contains the trigger distance (condition) between to actors
+ of a scenario
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - reference_actor: Reference CARLA actor
+ - name: Name of the condition
+ - distance: Trigger distance between the two actors in meters
+ - dx, dy, dz: distance to reference_location (location of reference_actor)
+
+ The condition terminates with SUCCESS, when the actor reached the target distance to the other actor
+ """
+
+ def __init__(self, reference_actor, actor, distance, comparison_operator=operator.lt,
+ name="TriggerDistanceToVehicle"):
+ """
+ Setup trigger distance
+ """
+ super(InTriggerDistanceToVehicle, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._reference_actor = reference_actor
+ self._actor = actor
+ self._distance = distance
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Check if the ego vehicle is within trigger distance to other actor
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ reference_location = CarlaDataProvider.get_location(self._reference_actor)
+
+ if location is None or reference_location is None:
+ return new_status
+
+ if self._comparison_operator(calculate_distance(location, reference_location), self._distance):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTriggerDistanceToLocation(AtomicCondition):
+
+ """
+ This class contains the trigger (condition) for a distance to a fixed
+ location of a scenario
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - target_location: Reference location (carla.location)
+ - name: Name of the condition
+ - distance: Trigger distance between the actor and the target location in meters
+
+ The condition terminates with SUCCESS, when the actor reached the target distance to the given location
+ """
+
+ def __init__(self,
+ actor,
+ target_location,
+ distance,
+ comparison_operator=operator.lt,
+ name="InTriggerDistanceToLocation"):
+ """
+ Setup trigger distance
+ """
+ super(InTriggerDistanceToLocation, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._target_location = target_location
+ self._actor = actor
+ self._distance = distance
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Check if the actor is within trigger distance to the target location
+ """
+
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+
+ if location is None:
+ return new_status
+
+ if self._comparison_operator(calculate_distance(
+ location, self._target_location), self._distance):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTriggerDistanceToNextIntersection(AtomicCondition):
+
+ """
+ This class contains the trigger (condition) for a distance to the
+ next intersection of a scenario
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - distance: Trigger distance between the actor and the next intersection in meters
+
+ The condition terminates with SUCCESS, when the actor reached the target distance to the next intersection
+ """
+
+ def __init__(self, actor, distance, name="InTriggerDistanceToNextIntersection"):
+ """
+ Setup trigger distance
+ """
+ super(InTriggerDistanceToNextIntersection, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._distance = distance
+ self._map = CarlaDataProvider.get_map()
+
+ waypoint = self._map.get_waypoint(self._actor.get_location())
+ while waypoint and not waypoint.is_intersection:
+ waypoint = waypoint.next(1)[-1]
+
+ self._final_location = waypoint.transform.location
+
+ def update(self):
+ """
+ Check if the actor is within trigger distance to the intersection
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ current_waypoint = self._map.get_waypoint(CarlaDataProvider.get_location(self._actor))
+ distance = calculate_distance(current_waypoint.transform.location, self._final_location)
+
+ if distance < self._distance:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTriggerDistanceToLocationAlongRoute(AtomicCondition):
+
+ """
+ Implementation for a behavior that will check if a given actor
+ is within a given distance to a given location considering a given route
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - distance: Trigger distance between the actor and the next intersection in meters
+ - route: Route to be checked
+ - location: Location on the route to be checked
+
+ The condition terminates with SUCCESS, when the actor reached the target distance
+ along its route to the given location
+ """
+
+ def __init__(self, actor, route, location, distance, name="InTriggerDistanceToLocationAlongRoute"):
+ """
+ Setup class members
+ """
+ super(InTriggerDistanceToLocationAlongRoute, self).__init__(name)
+ self._map = CarlaDataProvider.get_map()
+ self._actor = actor
+ self._location = location
+ self._route = route
+ self._distance = distance
+
+ self._location_distance, _ = get_distance_along_route(self._route, self._location)
+
+ def update(self):
+ new_status = py_trees.common.Status.RUNNING
+
+ current_location = CarlaDataProvider.get_location(self._actor)
+
+ if current_location is None:
+ return new_status
+
+ if current_location.distance(self._location) < self._distance + 20:
+
+ actor_distance, _ = get_distance_along_route(self._route, current_location)
+
+ # If closer than self._distance and hasn't passed the trigger point
+ if (self._location_distance < actor_distance + self._distance and
+ actor_distance < self._location_distance) or \
+ self._location_distance < 1.0:
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class InTimeToArrivalToLocation(AtomicCondition):
+
+ """
+ This class contains a check if a actor arrives within a given time
+ at a given location.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - time: The behavior is successful, if TTA is less than _time_ in seconds
+ - location: Location to be checked in this behavior
+
+ The condition terminates with SUCCESS, when the actor can reach the target location within the given time
+ """
+
+ _max_time_to_arrival = float('inf') # time to arrival in seconds
+
+ def __init__(self, actor, time, location, comparison_operator=operator.lt, name="TimeToArrival"):
+ """
+ Setup parameters
+ """
+ super(InTimeToArrivalToLocation, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._time = time
+ self._target_location = location
+ self._comparison_operator = comparison_operator
+
+ def update(self):
+ """
+ Check if the actor can arrive at target_location within time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ current_location = CarlaDataProvider.get_location(self._actor)
+
+ if current_location is None:
+ return new_status
+
+ distance = calculate_distance(current_location, self._target_location)
+ velocity = CarlaDataProvider.get_velocity(self._actor)
+
+ # if velocity is too small, simply use a large time to arrival
+ time_to_arrival = self._max_time_to_arrival
+ if velocity > EPSILON:
+ time_to_arrival = distance / velocity
+
+ if self._comparison_operator(time_to_arrival, self._time):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTimeToArrivalToVehicle(AtomicCondition):
+
+ """
+ This class contains a check if a actor arrives within a given time
+ at another actor.
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - time: The behavior is successful, if TTA is less than _time_ in seconds
+ - other_actor: Reference actor used in this behavior
+
+ The condition terminates with SUCCESS, when the actor can reach the other vehicle within the given time
+ """
+
+ _max_time_to_arrival = float('inf') # time to arrival in seconds
+
+ def __init__(self, actor, other_actor, time, along_route=False,
+ comparison_operator=operator.lt, name="TimeToArrival"):
+ """
+ Setup parameters
+ """
+ super(InTimeToArrivalToVehicle, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._map = CarlaDataProvider.get_map()
+ self._actor = actor
+ self._other_actor = other_actor
+ self._time = time
+ self._along_route = along_route
+ self._comparison_operator = comparison_operator
+
+ if self._along_route:
+ # Get the global route planner, used to calculate the route
+ dao = GlobalRoutePlannerDAO(self._map, 0.5)
+ grp = GlobalRoutePlanner(dao)
+ grp.setup()
+ self._grp = grp
+ else:
+ self._grp = None
+
+ def update(self):
+ """
+ Check if the ego vehicle can arrive at other actor within time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ current_location = CarlaDataProvider.get_location(self._actor)
+ other_location = CarlaDataProvider.get_location(self._other_actor)
+
+ if current_location is None or other_location is None:
+ return new_status
+
+ current_velocity = CarlaDataProvider.get_velocity(self._actor)
+ other_velocity = CarlaDataProvider.get_velocity(self._other_actor)
+
+ if self._along_route:
+ # Global planner needs a location at a driving lane
+ current_location = self._map.get_waypoint(current_location).transform.location
+ other_location = self._map.get_waypoint(other_location).transform.location
+
+ distance = calculate_distance(current_location, other_location, self._grp)
+
+ # if velocity is too small, simply use a large time to arrival
+ time_to_arrival = self._max_time_to_arrival
+
+ if current_velocity > other_velocity:
+ time_to_arrival = 2 * distance / (current_velocity - other_velocity)
+
+ if self._comparison_operator(time_to_arrival, self._time):
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class InTimeToArrivalToVehicleSideLane(InTimeToArrivalToLocation):
+
+ """
+ This class contains a check if a actor arrives within a given time
+ at another actor's side lane. Inherits from InTimeToArrivalToLocation
+
+ Important parameters:
+ - actor: CARLA actor to execute the behavior
+ - name: Name of the condition
+ - time: The behavior is successful, if TTA is less than _time_ in seconds
+ - cut_in_lane: the lane from where the other_actor will do the cut in
+ - other_actor: Reference actor used in this behavior
+
+ The condition terminates with SUCCESS, when the actor can reach the other vehicle within the given time
+ """
+
+ _max_time_to_arrival = float('inf') # time to arrival in seconds
+
+ def __init__(self, actor, other_actor, time, side_lane,
+ comparison_operator=operator.lt, name="InTimeToArrivalToVehicleSideLane"):
+ """
+ Setup parameters
+ """
+ self._other_actor = other_actor
+ self._side_lane = side_lane
+
+ self._world = CarlaDataProvider.get_world()
+ self._map = CarlaDataProvider.get_map(self._world)
+
+ other_location = other_actor.get_transform().location
+ other_waypoint = self._map.get_waypoint(other_location)
+
+ if self._side_lane == 'right':
+ other_side_waypoint = other_waypoint.get_left_lane()
+ elif self._side_lane == 'left':
+ other_side_waypoint = other_waypoint.get_right_lane()
+ else:
+ raise Exception("cut_in_lane must be either 'left' or 'right'")
+
+ other_side_location = other_side_waypoint.transform.location
+
+ super(InTimeToArrivalToVehicleSideLane, self).__init__(
+ actor, time, other_side_location, comparison_operator, name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+ """
+ Check if the ego vehicle can arrive at other actor within time
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ other_location = CarlaDataProvider.get_location(self._other_actor)
+ other_waypoint = self._map.get_waypoint(other_location)
+
+ if self._side_lane == 'right':
+ other_side_waypoint = other_waypoint.get_left_lane()
+ elif self._side_lane == 'left':
+ other_side_waypoint = other_waypoint.get_right_lane()
+ else:
+ raise Exception("cut_in_lane must be either 'left' or 'right'")
+ if other_side_waypoint is None:
+ return new_status
+
+ self._target_location = other_side_waypoint.transform.location
+
+ if self._target_location is None:
+ return new_status
+
+ new_status = super(InTimeToArrivalToVehicleSideLane, self).update()
+
+ return new_status
+
+
+class WaitUntilInFront(AtomicCondition):
+
+ """
+ Behavior that support the creation of cut ins. It waits until the actor has passed another actor
+
+ Parameters:
+ - actor: the one getting in front of the other actor
+ - other_actor: the reference vehicle that the actor will have to get in front of
+ - factor: How much in front the actor will have to get (from 0 to infinity):
+ 0: They are right next to each other
+ 1: The front of the other_actor and the back of the actor are right next to each other
+ """
+
+ def __init__(self, actor, other_actor, factor=1, check_distance=True, name="WaitUntilInFront"):
+ """
+ Init
+ """
+ super(WaitUntilInFront, self).__init__(name)
+ self._actor = actor
+ self._other_actor = other_actor
+ self._distance = 10
+ self._factor = max(EPSILON, factor) # Must be > 0
+ self._check_distance = check_distance
+
+ self._world = CarlaDataProvider.get_world()
+ self._map = CarlaDataProvider.get_map(self._world)
+
+ actor_extent = self._actor.bounding_box.extent.x
+ other_extent = self._other_actor.bounding_box.extent.x
+ self._length = self._factor * (actor_extent + other_extent)
+
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+ """
+ Checks if the two actors meet the requirements
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ in_front = False
+ close_by = False
+
+ # Location of our actor
+ actor_location = CarlaDataProvider.get_location(self._actor)
+ if actor_location is None:
+ return new_status
+
+ # Waypoint in front of the other actor
+ other_location = CarlaDataProvider.get_location(self._other_actor)
+ other_waypoint = self._map.get_waypoint(other_location)
+ if other_waypoint is None:
+ return new_status
+ other_next_waypoints = other_waypoint.next(self._length)
+ if other_next_waypoints is None:
+ return new_status
+ other_next_waypoint = other_next_waypoints[0]
+
+ # Wait for the vehicle to be in front
+ other_dir = other_next_waypoint.transform.get_forward_vector()
+ act_other_dir = actor_location - other_next_waypoint.transform.location
+ dot_ve_wp = other_dir.x * act_other_dir.x + other_dir.y * act_other_dir.y + other_dir.z * act_other_dir.z
+
+ if dot_ve_wp > 0.0:
+ in_front = True
+
+ # Wait for it to be close-by
+ if not self._check_distance:
+ close_by = True
+ elif actor_location.distance(other_next_waypoint.transform.location) < self._distance:
+ close_by = True
+
+ if in_front and close_by:
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class DriveDistance(AtomicCondition):
+
+ """
+ This class contains an atomic behavior to drive a certain distance.
+
+ Important parameters:
+ - actor: CARLA actor to execute the condition
+ - distance: Distance for this condition in meters
+
+ The condition terminates with SUCCESS, when the actor drove at least the given distance
+ """
+
+ def __init__(self, actor, distance, name="DriveDistance"):
+ """
+ Setup parameters
+ """
+ super(DriveDistance, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._target_distance = distance
+ self._distance = 0
+ self._location = None
+ self._actor = actor
+
+ def initialise(self):
+ self._location = CarlaDataProvider.get_location(self._actor)
+ super(DriveDistance, self).initialise()
+
+ def update(self):
+ """
+ Check driven distance
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ new_location = CarlaDataProvider.get_location(self._actor)
+ self._distance += calculate_distance(self._location, new_location)
+ self._location = new_location
+
+ if self._distance > self._target_distance:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+
+class AtRightmostLane(AtomicCondition):
+
+ """
+ This class contains an atomic behavior to check if the actor is at the rightest driving lane.
+
+ Important parameters:
+ - actor: CARLA actor to execute the condition
+
+ The condition terminates with SUCCESS, when the actor enters the rightest lane
+ """
+
+ def __init__(self, actor, name="AtRightmostLane"):
+ """
+ Setup parameters
+ """
+ super(AtRightmostLane, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor
+ self._map = CarlaDataProvider.get_map()
+
+ def update(self):
+ """
+ Check actor waypoints
+ """
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self._actor)
+ waypoint = self._map.get_waypoint(location)
+ if waypoint is None:
+ return new_status
+ right_waypoint = waypoint.get_right_lane()
+ if right_waypoint is None:
+ return new_status
+ lane_type = right_waypoint.lane_type
+
+ if lane_type != carla.LaneType.Driving:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+ return new_status
+
+
+class WaitForTrafficLightState(AtomicCondition):
+
+ """
+ This class contains an atomic behavior to wait for a given traffic light
+ to have the desired state.
+
+ Args:
+ actor (carla.TrafficLight): CARLA traffic light to execute the condition
+ state (carla.TrafficLightState): State to be checked in this condition
+
+ The condition terminates with SUCCESS, when the traffic light switches to the desired state
+ """
+
+ def __init__(self, actor, state, name="WaitForTrafficLightState"):
+ """
+ Setup traffic_light
+ """
+ super(WaitForTrafficLightState, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._actor = actor if "traffic_light" in actor.type_id else None
+ self._state = state
+
+ def update(self):
+ """
+ Set status to SUCCESS, when traffic light state matches the expected one
+ """
+ if self._actor is None:
+ return py_trees.common.Status.FAILURE
+
+ new_status = py_trees.common.Status.RUNNING
+
+ if self._actor.state == self._state:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class WaitEndIntersection(AtomicCondition):
+
+ """
+ Atomic behavior that waits until the vehicles has gone outside the junction.
+ If currently inside no intersection, it will wait until one is found
+ """
+
+ def __init__(self, actor, debug=False, name="WaitEndIntersection"):
+ super(WaitEndIntersection, self).__init__(name)
+ self.actor = actor
+ self.debug = debug
+ self.inside_junction = False
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ def update(self):
+
+ new_status = py_trees.common.Status.RUNNING
+
+ location = CarlaDataProvider.get_location(self.actor)
+ waypoint = CarlaDataProvider.get_map().get_waypoint(location)
+
+ # Wait for the actor to enter a junction
+ if not self.inside_junction and waypoint.is_junction:
+ self.inside_junction = True
+
+ # And to leave it
+ if self.inside_junction and not waypoint.is_junction:
+ if self.debug:
+ print("--- Leaving the junction")
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
+
+
+class WaitForBlackboardVariable(AtomicCondition):
+
+ """
+ Atomic behavior that keeps running until the blackboard variable is set to the corresponding value.
+ Used to avoid returning FAILURE if the blackboard comparison fails.
+
+ It also initially sets the variable to a given value, if given
+ """
+
+ def __init__(self, variable_name, variable_value, var_init_value=None,
+ debug=False, name="WaitForBlackboardVariable"):
+ super(WaitForBlackboardVariable, self).__init__(name)
+ self._debug = debug
+ self._variable_name = variable_name
+ self._variable_value = variable_value
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+
+ if var_init_value is not None:
+ blackboard = py_trees.blackboard.Blackboard()
+ _ = blackboard.set(variable_name, var_init_value)
+
+ def update(self):
+
+ new_status = py_trees.common.Status.RUNNING
+
+ blackv = py_trees.blackboard.Blackboard()
+ value = blackv.get(self._variable_name)
+ if value == self._variable_value:
+ if self._debug:
+ print("Blackboard variable {} set to True".format(self._variable_name))
+ new_status = py_trees.common.Status.SUCCESS
+
+ return new_status
diff --git a/scenario_runner/srunner/scenariomanager/timer.py b/scenario_runner/srunner/scenariomanager/timer.py
new file mode 100644
index 0000000..c79f50a
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/timer.py
@@ -0,0 +1,158 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides access to the CARLA game time and contains a py_trees
+timeout behavior using the CARLA game time
+"""
+
+import datetime
+import py_trees
+
+
+class GameTime(object):
+
+ """
+ This (static) class provides access to the CARLA game time.
+
+ The elapsed game time can be simply retrieved by calling:
+ GameTime.get_time()
+ """
+
+ _current_game_time = 0.0 # Elapsed game time after starting this Timer
+ _carla_time = 0.0
+ _last_frame = 0
+ _platform_timestamp = 0
+ _init = False
+
+ @staticmethod
+ def on_carla_tick(timestamp):
+ """
+ Callback receiving the CARLA time
+ Update time only when frame is more recent that last frame
+ """
+ if GameTime._last_frame < timestamp.frame:
+ frames = timestamp.frame - GameTime._last_frame if GameTime._init else 1
+ GameTime._current_game_time += timestamp.delta_seconds * frames
+ GameTime._last_frame = timestamp.frame
+ GameTime._platform_timestamp = datetime.datetime.now()
+ GameTime._init = True
+ GameTime._carla_time = timestamp.elapsed_seconds
+
+ @staticmethod
+ def restart():
+ """
+ Reset game timer to 0
+ """
+ GameTime._current_game_time = 0.0
+ GameTime._init = False
+
+ @staticmethod
+ def get_time():
+ """
+ Returns elapsed game time
+ """
+ return GameTime._current_game_time
+
+ @staticmethod
+ def get_carla_time():
+ """
+ Returns elapsed game time
+ """
+ return GameTime._carla_time
+
+ @staticmethod
+ def get_wallclocktime():
+ """
+ Returns elapsed game time
+ """
+ return GameTime._platform_timestamp
+
+ @staticmethod
+ def get_frame():
+ """
+ Returns elapsed game time
+ """
+ return GameTime._last_frame
+
+
+class SimulationTimeCondition(py_trees.behaviour.Behaviour):
+
+ """
+ This class contains an atomic simulation time condition behavior.
+ It uses the CARLA game time, not the system time which is used by
+ the py_trees timer.
+
+ Returns, if the provided success_rule (greaterThan, lessThan, equalTo)
+ was successfully evaluated
+ """
+
+ def __init__(self, timeout, success_rule="greaterThan", name="SimulationTimeCondition"):
+ """
+ Setup timeout
+ """
+ super(SimulationTimeCondition, self).__init__(name)
+ self.logger.debug("%s.__init__()" % (self.__class__.__name__))
+ self._timeout_value = timeout
+ self._start_time = 0.0
+ self._success_rule = success_rule
+ self._ops = {"greaterThan": (lambda x, y: x > y),
+ "equalTo": (lambda x, y: x == y),
+ "lessThan": (lambda x, y: x < y)}
+
+ def initialise(self):
+ """
+ Set start_time to current GameTime
+ """
+ self._start_time = GameTime.get_time()
+ self.logger.debug("%s.initialise()" % (self.__class__.__name__))
+
+ def update(self):
+ """
+ Get current game time, and compare it to the timeout value
+ Upon successfully comparison using the provided success_rule operator,
+ the status changes to SUCCESS
+ """
+
+ elapsed_time = GameTime.get_time() - self._start_time
+
+ if not self._ops[self._success_rule](elapsed_time, self._timeout_value):
+ new_status = py_trees.common.Status.RUNNING
+ else:
+ new_status = py_trees.common.Status.SUCCESS
+
+ self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status))
+
+ return new_status
+
+
+class TimeOut(SimulationTimeCondition):
+
+ """
+ This class contains an atomic timeout behavior.
+ It uses the CARLA game time, not the system time which is used by
+ the py_trees timer.
+ """
+
+ def __init__(self, timeout, name="TimeOut"):
+ """
+ Setup timeout
+ """
+ super(TimeOut, self).__init__(timeout, name=name)
+ self.timeout = False
+
+ def update(self):
+ """
+ Upon reaching the timeout value the status changes to SUCCESS
+ """
+
+ new_status = super(TimeOut, self).update()
+
+ if new_status == py_trees.common.Status.SUCCESS:
+ self.timeout = True
+
+ return new_status
diff --git a/scenario_runner/srunner/scenariomanager/traffic_events.py b/scenario_runner/srunner/scenariomanager/traffic_events.py
new file mode 100644
index 0000000..75f5e82
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/traffic_events.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Collection of TrafficEvents
+"""
+
+from enum import Enum
+
+
+class TrafficEventType(Enum):
+
+ """
+ This enum represents different traffic events that occur during driving.
+ """
+
+ NORMAL_DRIVING = 0
+ COLLISION_STATIC = 1
+ COLLISION_VEHICLE = 2
+ COLLISION_PEDESTRIAN = 3
+ ROUTE_DEVIATION = 4
+ ROUTE_COMPLETION = 5
+ ROUTE_COMPLETED = 6
+ TRAFFIC_LIGHT_INFRACTION = 7
+ WRONG_WAY_INFRACTION = 8
+ ON_SIDEWALK_INFRACTION = 9
+ STOP_INFRACTION = 10
+ OUTSIDE_LANE_INFRACTION = 11
+ OUTSIDE_ROUTE_LANES_INFRACTION = 12
+ VEHICLE_BLOCKED = 13
+
+
+class TrafficEvent(object):
+
+ """
+ TrafficEvent definition
+ """
+
+ def __init__(self, event_type, message=None, dictionary=None):
+ """
+ Initialize object
+
+ :param event_type: TrafficEventType defining the type of traffic event
+ :param message: optional message to inform users of the event
+ :param dictionary: optional dictionary with arbitrary keys and values
+ """
+ self._type = event_type
+ self._message = message
+ self._dict = dictionary
+
+ def get_type(self):
+ """
+ @return type
+ """
+ return self._type
+
+ def get_message(self):
+ """
+ @return message
+ """
+ if self._message:
+ return self._message
+
+ return ""
+
+ def set_message(self, message):
+ """
+ Set message
+ """
+ self._message = message
+
+ def get_dict(self):
+ """
+ @return dictionary
+ """
+ return self._dict
+
+ def set_dict(self, dictionary):
+ """
+ Set dictionary
+ """
+ self._dict = dictionary
diff --git a/scenario_runner/srunner/scenariomanager/watchdog.py b/scenario_runner/srunner/scenariomanager/watchdog.py
new file mode 100644
index 0000000..e544744
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/watchdog.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides a simple watchdog timer to detect timeouts
+It is for example used in the ScenarioManager
+"""
+from __future__ import print_function
+
+from threading import Timer
+try:
+ import thread
+except ImportError:
+ import _thread as thread
+
+
+class Watchdog(object):
+
+ """
+ Simple watchdog timer to detect timeouts
+
+ Args:
+ timeout (float): Timeout value of the watchdog [seconds].
+ If it is not reset before exceeding this value, a KayboardInterrupt is raised.
+
+ Attributes:
+ _timeout (float): Timeout value of the watchdog [seconds].
+ _failed (bool): True if watchdog exception occured, false otherwise
+ """
+
+ def __init__(self, timeout=1.0):
+ """
+ Class constructor
+ """
+ self._timeout = timeout + 1.0 # Let's add one second here to avoid overlap with other CARLA timeouts
+ self._failed = False
+ self._timer = None
+
+ def start(self):
+ """
+ Start the watchdog
+ """
+ self._timer = Timer(self._timeout, self._event)
+ self._timer.daemon = True
+ self._timer.start()
+
+ def update(self):
+ """
+ Reset watchdog.
+ """
+ self.stop()
+ self.start()
+
+ def _event(self):
+ """
+ This method is called when the timer triggers. A KayboardInterrupt
+ is generated on the main thread and the watchdog is stopped.
+ """
+ print('Watchdog exception - Timeout of {} seconds occured'.format(self._timeout))
+ self._failed = True
+ self.stop()
+ thread.interrupt_main()
+
+ def stop(self):
+ """
+ Stops the watchdog.
+ """
+ self._timer.cancel()
+
+ def get_status(self):
+ """
+ returns:
+ bool: False if watchdog exception occured, True otherwise
+ """
+ return not self._failed
diff --git a/scenario_runner/srunner/scenariomanager/weather_sim.py b/scenario_runner/srunner/scenariomanager/weather_sim.py
new file mode 100644
index 0000000..0fdcf0b
--- /dev/null
+++ b/scenario_runner/srunner/scenariomanager/weather_sim.py
@@ -0,0 +1,166 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides a weather class and py_trees behavior
+to simulate weather in CARLA according to the astronomic
+behavior of the sun.
+"""
+
+import datetime
+import math
+import operator
+
+import ephem
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import GameTime
+
+
+class Weather(object):
+
+ """
+ Class to simulate weather in CARLA according to the astronomic behavior of the sun
+
+ The sun position (azimuth and altitude angles) is obtained by calculating its
+ astronomic position for the CARLA reference position (x=0, y=0, z=0) using the ephem
+ library.
+
+ Args:
+ carla_weather (carla.WeatherParameters): Initial weather settings.
+ dtime (datetime): Initial date and time in UTC (required for animation only).
+ Defaults to None.
+ animation (bool): Flag to allow animating the sun position over time.
+ Defaults to False.
+
+ Attributes:
+ carla_weather (carla.WeatherParameters): Weather parameters for CARLA.
+ animation (bool): Flag to allow animating the sun position over time.
+ _sun (ephem.Sun): The sun as astronomic entity.
+ _observer_location (ephem.Observer): Holds the geographical position (lat/lon/altitude)
+ for which the sun position is obtained.
+ datetime (datetime): Date and time in UTC (required for animation only).
+ """
+
+ def __init__(self, carla_weather, dtime=None, animation=False):
+ """
+ Class constructor
+ """
+ self.carla_weather = carla_weather
+ self.animation = animation
+
+ self._sun = ephem.Sun() # pylint: disable=no-member
+ self._observer_location = ephem.Observer()
+ geo_location = CarlaDataProvider.get_map().transform_to_geolocation(carla.Location(0, 0, 0))
+ self._observer_location.lon = str(geo_location.longitude)
+ self._observer_location.lat = str(geo_location.latitude)
+
+ #@TODO This requires the time to be in UTC to be accurate
+ self.datetime = dtime
+ if self.datetime:
+ self._observer_location.date = self.datetime
+
+ self.update()
+
+ def update(self, delta_time=0):
+ """
+ If the weather animation is true, the new sun position is calculated w.r.t delta_time
+
+ Nothing happens if animation or datetime are None.
+
+ Args:
+ delta_time (float): Time passed since self.datetime [seconds].
+ """
+ if not self.animation or not self.datetime:
+ return
+
+ self.datetime = self.datetime + datetime.timedelta(seconds=delta_time)
+ self._observer_location.date = self.datetime
+
+ self._sun.compute(self._observer_location)
+ self.carla_weather.sun_altitude_angle = math.degrees(self._sun.alt)
+ self.carla_weather.sun_azimuth_angle = math.degrees(self._sun.az)
+
+
+class WeatherBehavior(py_trees.behaviour.Behaviour):
+
+ """
+ Atomic to read weather settings from the blackboard and apply these in CARLA.
+ Used in combination with UpdateWeather() to have a continuous weather simulation.
+
+ This behavior is always in a running state and must never terminate.
+ The user must not add this behavior. It is automatically added by the ScenarioManager.
+
+ This atomic also sets the datetime to blackboard variable, used by TimeOfDayComparison atomic
+
+ Args:
+ name (string): Name of the behavior.
+ Defaults to 'WeatherBehavior'.
+
+ Attributes:
+ _weather (srunner.scenariomanager.weather_sim.Weather): Weather settings.
+ _current_time (float): Current CARLA time [seconds].
+ """
+
+ def __init__(self, name="WeatherBehavior"):
+ """
+ Setup parameters
+ """
+ super(WeatherBehavior, self).__init__(name)
+ self._weather = None
+ self._current_time = None
+
+ def initialise(self):
+ """
+ Set current time to current CARLA time
+ """
+ self._current_time = GameTime.get_time()
+
+ def update(self):
+ """
+ Check if new weather settings are available on the blackboard, and if yes fetch these
+ into the _weather attribute.
+
+ Apply the weather settings from _weather to CARLA.
+
+ Note:
+ To minimize CARLA server interactions, the weather is only updated, when the blackboard
+ is updated, or if the weather animation flag is true. In the latter case, the update
+ frequency is 1 Hz.
+
+ returns:
+ py_trees.common.Status.RUNNING
+ """
+
+ weather = None
+
+ try:
+ check_weather = operator.attrgetter("CarlaWeather")
+ weather = check_weather(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+
+ if weather:
+ self._weather = weather
+ delattr(py_trees.blackboard.Blackboard(), "CarlaWeather")
+ CarlaDataProvider.get_world().set_weather(self._weather.carla_weather)
+ py_trees.blackboard.Blackboard().set("Datetime", self._weather.datetime, overwrite=True)
+
+ if self._weather and self._weather.animation:
+ new_time = GameTime.get_time()
+ delta_time = new_time - self._current_time
+
+ if delta_time > 1:
+ self._weather.update(delta_time)
+ self._current_time = new_time
+ CarlaDataProvider.get_world().set_weather(self._weather.carla_weather)
+
+ py_trees.blackboard.Blackboard().set("Datetime", self._weather.datetime, overwrite=True)
+
+ return py_trees.common.Status.RUNNING
diff --git a/scenario_runner/srunner/scenarios/__init__.py b/scenario_runner/srunner/scenarios/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/scenarios/background_activity.py b/scenario_runner/srunner/scenarios/background_activity.py
new file mode 100644
index 0000000..addb860
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/background_activity.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Scenario spawning elements to make the town dynamic and interesting
+"""
+
+import carla
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class BackgroundActivity(BasicScenario):
+
+ """
+ Implementation of a scenario to spawn a set of background actors,
+ and to remove traffic jams in background traffic
+
+ This is a single ego vehicle scenario
+ """
+
+ town_amount = {
+ 'Town01': 120,
+ 'Town02': 100,
+ 'Town03': 120,
+ 'Town04': 200,
+ 'Town05': 120,
+ 'Town06': 150,
+ 'Town07': 110,
+ 'Town08': 180,
+ 'Town09': 300,
+ 'Town10': 120,
+ }
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, timeout=35 * 60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self.config = config
+ self.debug = debug_mode
+
+ self.timeout = timeout # Timeout of scenario in seconds
+
+ super(BackgroundActivity, self).__init__("BackgroundActivity",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ terminate_on_failure=True,
+ criteria_enable=True)
+
+ def _initialize_actors(self, config):
+
+ town_name = config.town
+ if town_name in self.town_amount:
+ amount = self.town_amount[town_name]
+ else:
+ amount = 0
+
+ new_actors = CarlaDataProvider.request_new_batch_actors('vehicle.*',
+ amount,
+ carla.Transform(),
+ autopilot=True,
+ random_location=True,
+ rolename='background')
+
+ if new_actors is None:
+ raise Exception("Error: Unable to add the background activity, all spawn points were occupied")
+
+ for _actor in new_actors:
+ self.other_actors.append(_actor)
+
+ def _create_behavior(self):
+ """
+ Basic behavior do nothing, i.e. Idle
+ """
+ pass
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ pass
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/basic_scenario.py b/scenario_runner/srunner/scenarios/basic_scenario.py
new file mode 100644
index 0000000..f4b41f7
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/basic_scenario.py
@@ -0,0 +1,308 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provide BasicScenario, the basic class of all the scenarios.
+"""
+
+from __future__ import print_function
+
+import operator
+import py_trees
+
+import carla
+
+import srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions as conditions
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import TimeOut
+from srunner.scenariomanager.weather_sim import WeatherBehavior
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import UpdateAllActorControls
+
+
+class BasicScenario(object):
+
+ """
+ Base class for user-defined scenario
+ """
+
+ def __init__(self, name, ego_vehicles, config, world,
+ debug_mode=False, terminate_on_failure=False, criteria_enable=False):
+ """
+ Setup all relevant parameters and create scenario
+ and instantiate scenario manager
+ """
+ self.other_actors = []
+ if not self.timeout: # pylint: disable=access-member-before-definition
+ self.timeout = 60 # If no timeout was provided, set it to 60 seconds
+
+ self.criteria_list = [] # List of evaluation criteria
+ self.scenario = None
+
+ self.ego_vehicles = ego_vehicles
+ self.name = name
+ self.config = config
+ self.terminate_on_failure = terminate_on_failure
+
+ self._initialize_environment(world)
+
+ # Initializing adversarial actors
+ self._initialize_actors(config)
+ if CarlaDataProvider.is_sync_mode():
+ world.tick()
+ else:
+ world.wait_for_tick()
+
+ # Setup scenario
+ if debug_mode:
+ py_trees.logging.level = py_trees.logging.Level.DEBUG
+
+ behavior = self._create_behavior()
+
+ criteria = None
+ if criteria_enable:
+ criteria = self._create_test_criteria()
+
+ # Add a trigger condition for the behavior to ensure the behavior is only activated, when it is relevant
+ behavior_seq = py_trees.composites.Sequence()
+ trigger_behavior = self._setup_scenario_trigger(config)
+ if trigger_behavior:
+ behavior_seq.add_child(trigger_behavior)
+
+ if behavior is not None:
+ behavior_seq.add_child(behavior)
+ behavior_seq.name = behavior.name
+
+ end_behavior = self._setup_scenario_end(config)
+ if end_behavior:
+ behavior_seq.add_child(end_behavior)
+
+ self.scenario = Scenario(behavior_seq, criteria, self.name, self.timeout, self.terminate_on_failure)
+
+ def _initialize_environment(self, world):
+ """
+ Default initialization of weather and road friction.
+ Override this method in child class to provide custom initialization.
+ """
+
+ # Set the appropriate weather conditions
+ world.set_weather(self.config.weather)
+
+ # Set the appropriate road friction
+ if self.config.friction is not None:
+ friction_bp = world.get_blueprint_library().find('static.trigger.friction')
+ extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
+ friction_bp.set_attribute('friction', str(self.config.friction))
+ friction_bp.set_attribute('extent_x', str(extent.x))
+ friction_bp.set_attribute('extent_y', str(extent.y))
+ friction_bp.set_attribute('extent_z', str(extent.z))
+
+ # Spawn Trigger Friction
+ transform = carla.Transform()
+ transform.location = carla.Location(-10000.0, -10000.0, 0.0)
+ world.spawn_actor(friction_bp, transform)
+
+ def _initialize_actors(self, config):
+ """
+ Default initialization of other actors.
+ Override this method in child class to provide custom initialization.
+ """
+ if config.other_actors:
+ new_actors = CarlaDataProvider.request_new_actors(config.other_actors)
+ if not new_actors:
+ raise Exception("Error: Unable to add actors")
+
+ for new_actor in new_actors:
+ self.other_actors.append(new_actor)
+
+ def _setup_scenario_trigger(self, config):
+ """
+ This function creates a trigger maneuver, that has to be finished before the real scenario starts.
+ This implementation focuses on the first available ego vehicle.
+
+ The function can be overloaded by a user implementation inside the user-defined scenario class.
+ """
+ start_location = None
+ if config.trigger_points and config.trigger_points[0]:
+ start_location = config.trigger_points[0].location # start location of the scenario
+
+ ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ if start_location:
+ if ego_vehicle_route:
+ if config.route_var_name is None: # pylint: disable=no-else-return
+ return conditions.InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
+ ego_vehicle_route,
+ start_location,
+ 5)
+ else:
+ check_name = "WaitForBlackboardVariable: {}".format(config.route_var_name)
+ return conditions.WaitForBlackboardVariable(name=check_name,
+ variable_name=config.route_var_name,
+ variable_value=True,
+ var_init_value=False)
+
+ return conditions.InTimeToArrivalToLocation(self.ego_vehicles[0],
+ 2.0,
+ start_location)
+
+ return None
+
+ def _setup_scenario_end(self, config):
+ """
+ This function adds and additional behavior to the scenario, which is triggered
+ after it has ended.
+
+ The function can be overloaded by a user implementation inside the user-defined scenario class.
+ """
+ ego_vehicle_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ if ego_vehicle_route:
+ if config.route_var_name is not None:
+ set_name = "Reset Blackboard Variable: {} ".format(config.route_var_name)
+ return py_trees.blackboard.SetBlackboardVariable(name=set_name,
+ variable_name=config.route_var_name,
+ variable_value=False)
+ return None
+
+ def _create_behavior(self):
+ """
+ Pure virtual function to setup user-defined scenario behavior
+ """
+ raise NotImplementedError(
+ "This function is re-implemented by all scenarios"
+ "If this error becomes visible the class hierarchy is somehow broken")
+
+ def _create_test_criteria(self):
+ """
+ Pure virtual function to setup user-defined evaluation criteria for the
+ scenario
+ """
+ raise NotImplementedError(
+ "This function is re-implemented by all scenarios"
+ "If this error becomes visible the class hierarchy is somehow broken")
+
+ def change_control(self, control): # pylint: disable=no-self-use
+ """
+ This is a function that changes the control based on the scenario determination
+ :param control: a carla vehicle control
+ :return: a control to be changed by the scenario.
+
+ Note: This method should be overriden by the user-defined scenario behavior
+ """
+ return control
+
+ def remove_all_actors(self):
+ """
+ Remove all actors
+ """
+ for i, _ in enumerate(self.other_actors):
+ if self.other_actors[i] is not None:
+ if CarlaDataProvider.actor_id_exists(self.other_actors[i].id):
+ CarlaDataProvider.remove_actor_by_id(self.other_actors[i].id)
+ self.other_actors[i] = None
+ self.other_actors = []
+
+
+class Scenario(object):
+
+ """
+ Basic scenario class. This class holds the behavior_tree describing the
+ scenario and the test criteria.
+
+ The user must not modify this class.
+
+ Important parameters:
+ - behavior: User defined scenario with py_tree
+ - criteria_list: List of user defined test criteria with py_tree
+ - timeout (default = 60s): Timeout of the scenario in seconds
+ - terminate_on_failure: Terminate scenario on first failure
+ """
+
+ def __init__(self, behavior, criteria, name, timeout=60, terminate_on_failure=False):
+ self.behavior = behavior
+ self.test_criteria = criteria
+ self.timeout = timeout
+ self.name = name
+
+ if self.test_criteria is not None and not isinstance(self.test_criteria, py_trees.composites.Parallel):
+ # list of nodes
+ for criterion in self.test_criteria:
+ criterion.terminate_on_failure = terminate_on_failure
+
+ # Create py_tree for test criteria
+ self.criteria_tree = py_trees.composites.Parallel(
+ name="Test Criteria",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE
+ )
+ self.criteria_tree.add_children(self.test_criteria)
+ self.criteria_tree.setup(timeout=1)
+ else:
+ self.criteria_tree = criteria
+
+ # Create node for timeout
+ self.timeout_node = TimeOut(self.timeout, name="TimeOut")
+
+ # Create overall py_tree
+ self.scenario_tree = py_trees.composites.Parallel(name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ if behavior is not None:
+ self.scenario_tree.add_child(self.behavior)
+ self.scenario_tree.add_child(self.timeout_node)
+ self.scenario_tree.add_child(WeatherBehavior())
+ self.scenario_tree.add_child(UpdateAllActorControls())
+
+ if criteria is not None:
+ self.scenario_tree.add_child(self.criteria_tree)
+ self.scenario_tree.setup(timeout=1)
+
+ def _extract_nodes_from_tree(self, tree): # pylint: disable=no-self-use
+ """
+ Returns the list of all nodes from the given tree
+ """
+ node_list = [tree]
+ more_nodes_exist = True
+ while more_nodes_exist:
+ more_nodes_exist = False
+ for node in node_list:
+ if node.children:
+ node_list.remove(node)
+ more_nodes_exist = True
+ for child in node.children:
+ node_list.append(child)
+
+ if len(node_list) == 1 and isinstance(node_list[0], py_trees.composites.Parallel):
+ return []
+
+ return node_list
+
+ def get_criteria(self):
+ """
+ Return the list of test criteria (all leave nodes)
+ """
+ criteria_list = self._extract_nodes_from_tree(self.criteria_tree)
+ return criteria_list
+
+ def terminate(self):
+ """
+ This function sets the status of all leaves in the scenario tree to INVALID
+ """
+ # Get list of all nodes in the tree
+ node_list = self._extract_nodes_from_tree(self.scenario_tree)
+
+ # Set status to INVALID
+ for node in node_list:
+ node.terminate(py_trees.common.Status.INVALID)
+
+ # Cleanup all instantiated controllers
+ actor_dict = {}
+ try:
+ check_actors = operator.attrgetter("ActorsWithController")
+ actor_dict = check_actors(py_trees.blackboard.Blackboard())
+ except AttributeError:
+ pass
+ for actor_id in actor_dict:
+ actor_dict[actor_id].reset()
+ py_trees.blackboard.Blackboard().set("ActorsWithController", {}, overwrite=True)
diff --git a/scenario_runner/srunner/scenarios/change_lane.py b/scenario_runner/srunner/scenarios/change_lane.py
new file mode 100644
index 0000000..b9ee80a
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/change_lane.py
@@ -0,0 +1,181 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Change lane scenario:
+
+The scenario realizes a driving behavior, in which the user-controlled ego vehicle
+follows a fast driving car on the highway. There's a slow car driving in great distance to the fast vehicle.
+At one point the fast vehicle is changing the lane to overtake a slow car, which is driving on the same lane.
+
+The ego vehicle doesn't "see" the slow car before the lane change of the fast car, therefore it hast to react
+fast to avoid an collision. There are two options to avoid an accident:
+The ego vehicle adjusts its velocity or changes the lane as well.
+"""
+
+import random
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ StopVehicle,
+ LaneChange,
+ WaypointFollower,
+ Idle)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle, StandStill
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_waypoint_in_distance
+
+
+class ChangeLane(BasicScenario):
+
+ """
+ This class holds everything required for a "change lane" scenario involving three vehicles.
+ There are two vehicles driving in the same direction on the highway: A fast car and a slow car in front.
+ The fast car will change the lane, when it is close to the slow car.
+
+ The ego vehicle is driving right behind the fast car.
+
+ This is a single ego vehicle scenario
+ """
+
+ timeout = 1200
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=600):
+ """
+ Setup all relevant parameters and create scenario
+
+ If randomize is True, the scenario parameters are randomized
+ """
+ self.timeout = timeout
+ self._map = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+
+ self._fast_vehicle_velocity = 70
+ self._slow_vehicle_velocity = 0
+ self._change_lane_velocity = 15
+
+ self._slow_vehicle_distance = 100
+ self._fast_vehicle_distance = 20
+ self._trigger_distance = 30
+ self._max_brake = 1
+
+ self.direction = 'left' # direction of lane change
+ self.lane_check = 'true' # check whether a lane change is possible
+
+ super(ChangeLane, self).__init__("ChangeLane",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ if randomize:
+ self._fast_vehicle_distance = random.randint(10, 51)
+ self._fast_vehicle_velocity = random.randint(100, 201)
+ self._slow_vehicle_velocity = random.randint(1, 6)
+
+ def _initialize_actors(self, config):
+
+ # add actors from xml file
+ for actor in config.other_actors:
+ vehicle = CarlaDataProvider.request_new_actor(actor.model, actor.transform)
+ self.other_actors.append(vehicle)
+ vehicle.set_simulate_physics(enabled=False)
+
+ # fast vehicle, tesla
+ # transform visible
+ fast_car_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._fast_vehicle_distance)
+ self.fast_car_visible = carla.Transform(
+ carla.Location(fast_car_waypoint.transform.location.x,
+ fast_car_waypoint.transform.location.y,
+ fast_car_waypoint.transform.location.z + 1),
+ fast_car_waypoint.transform.rotation)
+
+ # slow vehicle, vw
+ # transform visible
+ slow_car_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._slow_vehicle_distance)
+ self.slow_car_visible = carla.Transform(
+ carla.Location(slow_car_waypoint.transform.location.x,
+ slow_car_waypoint.transform.location.y,
+ slow_car_waypoint.transform.location.z),
+ slow_car_waypoint.transform.rotation)
+
+ def _create_behavior(self):
+
+ # sequence vw
+ # make visible
+ sequence_vw = py_trees.composites.Sequence("VW T2")
+ vw_visible = ActorTransformSetter(self.other_actors[1], self.slow_car_visible)
+ sequence_vw.add_child(vw_visible)
+
+ # brake, avoid rolling backwarts
+ brake = StopVehicle(self.other_actors[1], self._max_brake)
+ sequence_vw.add_child(brake)
+ sequence_vw.add_child(Idle())
+
+ # sequence tesla
+ # make visible
+ sequence_tesla = py_trees.composites.Sequence("Tesla")
+ tesla_visible = ActorTransformSetter(self.other_actors[0], self.fast_car_visible)
+ sequence_tesla.add_child(tesla_visible)
+
+ # drive fast towards slow vehicle
+ just_drive = py_trees.composites.Parallel("DrivingTowardsSlowVehicle",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ tesla_driving_fast = WaypointFollower(self.other_actors[0], self._fast_vehicle_velocity)
+ just_drive.add_child(tesla_driving_fast)
+ distance_to_vehicle = InTriggerDistanceToVehicle(
+ self.other_actors[1], self.other_actors[0], self._trigger_distance)
+ just_drive.add_child(distance_to_vehicle)
+ sequence_tesla.add_child(just_drive)
+
+ # change lane
+ lane_change_atomic = LaneChange(self.other_actors[0], distance_other_lane=200)
+ sequence_tesla.add_child(lane_change_atomic)
+ sequence_tesla.add_child(Idle())
+
+ # ego vehicle
+ # end condition
+ endcondition = py_trees.composites.Parallel("Waiting for end position of ego vehicle",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+ endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[1],
+ self.ego_vehicles[0],
+ distance=20,
+ name="FinalDistance")
+ endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
+ endcondition.add_child(endcondition_part1)
+ endcondition.add_child(endcondition_part2)
+
+ # build tree
+ root = py_trees.composites.Parallel("Parallel Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ root.add_child(sequence_vw)
+ root.add_child(sequence_tesla)
+ root.add_child(endcondition)
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/control_loss.py b/scenario_runner/srunner/scenarios/control_loss.py
new file mode 100644
index 0000000..0742b1b
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/control_loss.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Control Loss Vehicle scenario:
+
+The scenario realizes that the vehicle looses control due to
+bad road conditions, etc. and checks to see if the vehicle
+regains control and corrects it's course.
+"""
+
+import numpy.random as random
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeNoiseParameters, ActorTransformSetter
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation,
+ InTriggerDistanceToNextIntersection,
+ DriveDistance)
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_location_in_distance_from_wp
+
+
+class ControlLoss(BasicScenario):
+
+ """
+ Implementation of "Control Loss Vehicle" (Traffic Scenario 01)
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ # ego vehicle parameters
+ self._no_of_jitter = 10
+ self._noise_mean = 0 # Mean value of steering noise
+ self._noise_std = 0.01 # Std. deviation of steering noise
+ self._dynamic_mean_for_steer = 0.001
+ self._dynamic_mean_for_throttle = 0.045
+ self._abort_distance_to_intersection = 10
+ self._current_steer_noise = [0] # This is a list, since lists are mutable
+ self._current_throttle_noise = [0]
+ self._start_distance = 20
+ self._trigger_dist = 2
+ self._end_distance = 30
+ self._ego_vehicle_max_steer = 0.0
+ self._ego_vehicle_max_throttle = 1.0
+ self._ego_vehicle_target_velocity = 15
+ self._map = CarlaDataProvider.get_map()
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ # The reference trigger for the control loss
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+ self.loc_list = []
+ self.obj = []
+ self._randomize = randomize
+ super(ControlLoss, self).__init__("ControlLoss",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ if self._randomize:
+ self._distance = random.randint(low=10, high=80, size=3)
+ self._distance = sorted(self._distance)
+ else:
+ self._distance = [14, 48, 74]
+ first_loc, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._distance[0])
+ second_loc, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._distance[1])
+ third_loc, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._distance[2])
+
+ self.loc_list.extend([first_loc, second_loc, third_loc])
+ self._dist_prop = [x - 2 for x in self._distance]
+
+ self.first_loc_prev, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._dist_prop[0])
+ self.sec_loc_prev, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._dist_prop[1])
+ self.third_loc_prev, _ = get_location_in_distance_from_wp(self._reference_waypoint, self._dist_prop[2])
+
+ self.first_transform = carla.Transform(self.first_loc_prev)
+ self.sec_transform = carla.Transform(self.sec_loc_prev)
+ self.third_transform = carla.Transform(self.third_loc_prev)
+ self.first_transform = carla.Transform(carla.Location(self.first_loc_prev.x,
+ self.first_loc_prev.y,
+ self.first_loc_prev.z))
+ self.sec_transform = carla.Transform(carla.Location(self.sec_loc_prev.x,
+ self.sec_loc_prev.y,
+ self.sec_loc_prev.z))
+ self.third_transform = carla.Transform(carla.Location(self.third_loc_prev.x,
+ self.third_loc_prev.y,
+ self.third_loc_prev.z))
+
+ first_debris = CarlaDataProvider.request_new_actor('static.prop.dirtdebris01', self.first_transform, 'prop')
+ second_debris = CarlaDataProvider.request_new_actor('static.prop.dirtdebris01', self.sec_transform, 'prop')
+ third_debris = CarlaDataProvider.request_new_actor('static.prop.dirtdebris01', self.third_transform, 'prop')
+
+ first_debris.set_transform(self.first_transform)
+ second_debris.set_transform(self.sec_transform)
+ third_debris.set_transform(self.third_transform)
+
+ self.obj.extend([first_debris, second_debris, third_debris])
+ for debris in self.obj:
+ debris.set_simulate_physics(False)
+
+ self.other_actors.append(first_debris)
+ self.other_actors.append(second_debris)
+ self.other_actors.append(third_debris)
+
+ def _create_behavior(self):
+ """
+ The scenario defined after is a "control loss vehicle" scenario. After
+ invoking this scenario, it will wait until the vehicle drove a few meters
+ (_start_distance), and then perform a jitter action. Finally, the vehicle
+ has to reach a target point (_end_distance). If this does not happen within
+ 60 seconds, a timeout stops the scenario
+ """
+ # start condition
+ start_end_parallel = py_trees.composites.Parallel("Jitter",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ start_condition = InTriggerDistanceToLocation(self.ego_vehicles[0], self.first_loc_prev, self._trigger_dist)
+ for _ in range(self._no_of_jitter):
+
+ # change the current noise to be applied
+ turn = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise,
+ self._noise_mean, self._noise_std, self._dynamic_mean_for_steer,
+ self._dynamic_mean_for_throttle) # Mean value of steering noise
+ # Noise end! put again the added noise to zero.
+ noise_end = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise,
+ 0, 0, 0, 0)
+
+ jitter_action = py_trees.composites.Parallel("Jitter",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ # Abort jitter_sequence, if the vehicle is approaching an intersection
+ jitter_abort = InTriggerDistanceToNextIntersection(self.ego_vehicles[0], self._abort_distance_to_intersection)
+ # endcondition: Check if vehicle reached waypoint _end_distance from here:
+ end_condition = DriveDistance(self.ego_vehicles[0], self._end_distance)
+ start_end_parallel.add_child(start_condition)
+ start_end_parallel.add_child(end_condition)
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("ControlLoss")
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self.first_transform, physics=False))
+ sequence.add_child(ActorTransformSetter(self.other_actors[1], self.sec_transform, physics=False))
+ sequence.add_child(ActorTransformSetter(self.other_actors[2], self.third_transform, physics=False))
+ jitter = py_trees.composites.Sequence("Jitter Behavior")
+ jitter.add_child(turn)
+ jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.sec_loc_prev, self._trigger_dist))
+ jitter.add_child(turn)
+ jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.third_loc_prev, self._trigger_dist))
+ jitter.add_child(turn)
+ jitter_action.add_child(jitter)
+ jitter_action.add_child(jitter_abort)
+ sequence.add_child(start_end_parallel)
+ sequence.add_child(jitter_action)
+ sequence.add_child(end_condition)
+ sequence.add_child(noise_end)
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def change_control(self, control):
+ """
+ This is a function that changes the control based on the scenario determination
+ :param control: a carla vehicle control
+ :return: a control to be changed by the scenario.
+ """
+ control.steer += self._current_steer_noise[0]
+ control.throttle += self._current_throttle_noise[0]
+
+ return control
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/cut_in.py b/scenario_runner/srunner/scenarios/cut_in.py
new file mode 100644
index 0000000..9de4416
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/cut_in.py
@@ -0,0 +1,158 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Cut in scenario:
+
+The scenario realizes a driving behavior on the highway.
+The user-controlled ego vehicle is driving straight and keeping its velocity at a constant level.
+Another car is cutting just in front, coming from left or right lane.
+
+The ego vehicle may need to brake to avoid a collision.
+"""
+
+import random
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ LaneChange,
+ WaypointFollower,
+ AccelerateToCatchUp)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerDistanceToVehicle, DriveDistance
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class CutIn(BasicScenario):
+
+ """
+ The ego vehicle is driving on a highway and another car is cutting in just in front.
+ This is a single ego vehicle scenario
+ """
+
+ timeout = 1200
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=600):
+
+ self.timeout = timeout
+ self._map = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+
+ self._velocity = 40
+ self._delta_velocity = 10
+ self._trigger_distance = 30
+
+ # get direction from config name
+ self._config = config
+ self._direction = None
+ self._transform_visible = None
+
+ super(CutIn, self).__init__("CutIn",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ if randomize:
+ self._velocity = random.randint(20, 60)
+ self._trigger_distance = random.randint(10, 40)
+
+ def _initialize_actors(self, config):
+
+ # direction of lane, on which other_actor is driving before lane change
+ if 'LEFT' in self._config.name.upper():
+ self._direction = 'left'
+
+ if 'RIGHT' in self._config.name.upper():
+ self._direction = 'right'
+
+ # add actors from xml file
+ for actor in config.other_actors:
+ vehicle = CarlaDataProvider.request_new_actor(actor.model, actor.transform)
+ self.other_actors.append(vehicle)
+ vehicle.set_simulate_physics(enabled=False)
+
+ # transform visible
+ other_actor_transform = self.other_actors[0].get_transform()
+ self._transform_visible = carla.Transform(
+ carla.Location(other_actor_transform.location.x,
+ other_actor_transform.location.y,
+ other_actor_transform.location.z + 105),
+ other_actor_transform.rotation)
+
+ def _create_behavior(self):
+ """
+ Order of sequence:
+ - car_visible: spawn car at a visible transform
+ - just_drive: drive until in trigger distance to ego_vehicle
+ - accelerate: accelerate to catch up distance to ego_vehicle
+ - lane_change: change the lane
+ - endcondition: drive for a defined distance
+ """
+
+ # car_visible
+ behaviour = py_trees.composites.Sequence("CarOn_{}_Lane" .format(self._direction))
+ car_visible = ActorTransformSetter(self.other_actors[0], self._transform_visible)
+ behaviour.add_child(car_visible)
+
+ # just_drive
+ just_drive = py_trees.composites.Parallel(
+ "DrivingStraight", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ car_driving = WaypointFollower(self.other_actors[0], self._velocity)
+ just_drive.add_child(car_driving)
+
+ trigger_distance = InTriggerDistanceToVehicle(
+ self.other_actors[0], self.ego_vehicles[0], self._trigger_distance)
+ just_drive.add_child(trigger_distance)
+ behaviour.add_child(just_drive)
+
+ # accelerate
+ accelerate = AccelerateToCatchUp(self.other_actors[0], self.ego_vehicles[0], throttle_value=1,
+ delta_velocity=self._delta_velocity, trigger_distance=5, max_distance=500)
+ behaviour.add_child(accelerate)
+
+ # lane_change
+ if self._direction == 'left':
+ lane_change = LaneChange(
+ self.other_actors[0], speed=None, direction='right', distance_same_lane=5, distance_other_lane=300)
+ behaviour.add_child(lane_change)
+ else:
+ lane_change = LaneChange(
+ self.other_actors[0], speed=None, direction='left', distance_same_lane=5, distance_other_lane=300)
+ behaviour.add_child(lane_change)
+
+ # endcondition
+ endcondition = DriveDistance(self.other_actors[0], 200)
+
+ # build tree
+ root = py_trees.composites.Sequence("Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ root.add_child(behaviour)
+ root.add_child(endcondition)
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria is created, which is later used in the parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors after deletion.
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/follow_leading_vehicle.py b/scenario_runner/srunner/scenarios/follow_leading_vehicle.py
new file mode 100644
index 0000000..b4c6d05
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/follow_leading_vehicle.py
@@ -0,0 +1,325 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Follow leading vehicle scenario:
+
+The scenario realizes a common driving behavior, in which the
+user-controlled ego vehicle follows a leading car driving down
+a given road. At some point the leading car has to slow down and
+finally stop. The ego vehicle has to react accordingly to avoid
+a collision. The scenario ends either via a timeout, or if the ego
+vehicle stopped close enough to the leading vehicle
+"""
+
+import random
+
+import py_trees
+
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ KeepVelocity,
+ StopVehicle,
+ WaypointFollower)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToVehicle,
+ InTriggerDistanceToNextIntersection,
+ DriveDistance,
+ StandStill)
+from srunner.scenariomanager.timer import TimeOut
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_waypoint_in_distance
+
+
+class FollowLeadingVehicle(BasicScenario):
+
+ """
+ This class holds everything required for a simple "Follow a leading vehicle"
+ scenario involving two vehicles. (Traffic Scenario 2)
+
+ This is a single ego vehicle scenario
+ """
+
+ timeout = 120 # Timeout of scenario in seconds
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+
+ If randomize is True, the scenario parameters are randomized
+ """
+
+ self._map = CarlaDataProvider.get_map()
+ self._first_vehicle_location = 25
+ self._first_vehicle_speed = 10
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+ self._other_actor_max_brake = 1.0
+ self._other_actor_stop_in_front_intersection = 20
+ self._other_actor_transform = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(FollowLeadingVehicle, self).__init__("FollowVehicle",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ if randomize:
+ self._ego_other_distance_start = random.randint(4, 8)
+
+ # Example code how to randomize start location
+ # distance = random.randint(20, 80)
+ # new_location, _ = get_location_in_distance(self.ego_vehicles[0], distance)
+ # waypoint = CarlaDataProvider.get_map().get_waypoint(new_location)
+ # waypoint.transform.location.z += 39
+ # self.other_actors[0].set_transform(waypoint.transform)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ first_vehicle_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._first_vehicle_location)
+ self._other_actor_transform = carla.Transform(
+ carla.Location(first_vehicle_waypoint.transform.location.x,
+ first_vehicle_waypoint.transform.location.y,
+ first_vehicle_waypoint.transform.location.z + 1),
+ first_vehicle_waypoint.transform.rotation)
+ first_vehicle_transform = carla.Transform(
+ carla.Location(self._other_actor_transform.location.x,
+ self._other_actor_transform.location.y,
+ self._other_actor_transform.location.z - 500),
+ self._other_actor_transform.rotation)
+ first_vehicle = CarlaDataProvider.request_new_actor('vehicle.nissan.patrol', first_vehicle_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ The scenario defined after is a "follow leading vehicle" scenario. After
+ invoking this scenario, it will wait for the user controlled vehicle to
+ enter the start region, then make the other actor to drive until reaching
+ the next intersection. Finally, the user-controlled vehicle has to be close
+ enough to the other actor to end the scenario.
+ If this does not happen within 60 seconds, a timeout stops the scenario
+ """
+
+ # to avoid the other actor blocking traffic, it was spawed elsewhere
+ # reset its pose to the required one
+ start_transform = ActorTransformSetter(self.other_actors[0], self._other_actor_transform)
+
+ # let the other actor drive until next intersection
+ # @todo: We should add some feedback mechanism to respond to ego_vehicle behavior
+ driving_to_next_intersection = py_trees.composites.Parallel(
+ "DrivingTowardsIntersection",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ driving_to_next_intersection.add_child(WaypointFollower(self.other_actors[0], self._first_vehicle_speed))
+ driving_to_next_intersection.add_child(InTriggerDistanceToNextIntersection(
+ self.other_actors[0], self._other_actor_stop_in_front_intersection))
+
+ # stop vehicle
+ stop = StopVehicle(self.other_actors[0], self._other_actor_max_brake)
+
+ # end condition
+ endcondition = py_trees.composites.Parallel("Waiting for end position",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+ endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
+ self.ego_vehicles[0],
+ distance=20,
+ name="FinalDistance")
+ endcondition_part2 = StandStill(self.ego_vehicles[0], name="StandStill", duration=1)
+ endcondition.add_child(endcondition_part1)
+ endcondition.add_child(endcondition_part2)
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("Sequence Behavior")
+ sequence.add_child(start_transform)
+ sequence.add_child(driving_to_next_intersection)
+ sequence.add_child(stop)
+ sequence.add_child(endcondition)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
+
+
+class FollowLeadingVehicleWithObstacle(BasicScenario):
+
+ """
+ This class holds a scenario similar to FollowLeadingVehicle
+ but there is an obstacle in front of the leading vehicle
+
+ This is a single ego vehicle scenario
+ """
+
+ timeout = 120 # Timeout of scenario in seconds
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._map = CarlaDataProvider.get_map()
+ self._first_actor_location = 25
+ self._second_actor_location = self._first_actor_location + 41
+ self._first_actor_speed = 10
+ self._second_actor_speed = 1.5
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+ self._other_actor_max_brake = 1.0
+ self._first_actor_transform = None
+ self._second_actor_transform = None
+
+ super(FollowLeadingVehicleWithObstacle, self).__init__("FollowLeadingVehicleWithObstacle",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+ if randomize:
+ self._ego_other_distance_start = random.randint(4, 8)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ first_actor_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._first_actor_location)
+ second_actor_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._second_actor_location)
+ first_actor_transform = carla.Transform(
+ carla.Location(first_actor_waypoint.transform.location.x,
+ first_actor_waypoint.transform.location.y,
+ first_actor_waypoint.transform.location.z - 500),
+ first_actor_waypoint.transform.rotation)
+ self._first_actor_transform = carla.Transform(
+ carla.Location(first_actor_waypoint.transform.location.x,
+ first_actor_waypoint.transform.location.y,
+ first_actor_waypoint.transform.location.z + 1),
+ first_actor_waypoint.transform.rotation)
+ yaw_1 = second_actor_waypoint.transform.rotation.yaw + 90
+ second_actor_transform = carla.Transform(
+ carla.Location(second_actor_waypoint.transform.location.x,
+ second_actor_waypoint.transform.location.y,
+ second_actor_waypoint.transform.location.z - 500),
+ carla.Rotation(second_actor_waypoint.transform.rotation.pitch, yaw_1,
+ second_actor_waypoint.transform.rotation.roll))
+ self._second_actor_transform = carla.Transform(
+ carla.Location(second_actor_waypoint.transform.location.x,
+ second_actor_waypoint.transform.location.y,
+ second_actor_waypoint.transform.location.z + 1),
+ carla.Rotation(second_actor_waypoint.transform.rotation.pitch, yaw_1,
+ second_actor_waypoint.transform.rotation.roll))
+
+ first_actor = CarlaDataProvider.request_new_actor(
+ 'vehicle.nissan.patrol', first_actor_transform)
+ second_actor = CarlaDataProvider.request_new_actor(
+ 'vehicle.diamondback.century', second_actor_transform)
+
+ first_actor.set_simulate_physics(enabled=False)
+ second_actor.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_actor)
+ self.other_actors.append(second_actor)
+
+ def _create_behavior(self):
+ """
+ The scenario defined after is a "follow leading vehicle" scenario. After
+ invoking this scenario, it will wait for the user controlled vehicle to
+ enter the start region, then make the other actor to drive towards obstacle.
+ Once obstacle clears the road, make the other actor to drive towards the
+ next intersection. Finally, the user-controlled vehicle has to be close
+ enough to the other actor to end the scenario.
+ If this does not happen within 60 seconds, a timeout stops the scenario
+ """
+
+ # let the other actor drive until next intersection
+ driving_to_next_intersection = py_trees.composites.Parallel(
+ "Driving towards Intersection",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ obstacle_clear_road = py_trees.composites.Parallel("Obstalce clearing road",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ obstacle_clear_road.add_child(DriveDistance(self.other_actors[1], 4))
+ obstacle_clear_road.add_child(KeepVelocity(self.other_actors[1], self._second_actor_speed))
+
+ stop_near_intersection = py_trees.composites.Parallel(
+ "Waiting for end position near Intersection",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ stop_near_intersection.add_child(WaypointFollower(self.other_actors[0], 10))
+ stop_near_intersection.add_child(InTriggerDistanceToNextIntersection(self.other_actors[0], 20))
+
+ driving_to_next_intersection.add_child(WaypointFollower(self.other_actors[0], self._first_actor_speed))
+ driving_to_next_intersection.add_child(InTriggerDistanceToVehicle(self.other_actors[1],
+ self.other_actors[0], 15))
+
+ # end condition
+ endcondition = py_trees.composites.Parallel("Waiting for end position",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+ endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
+ self.ego_vehicles[0],
+ distance=20,
+ name="FinalDistance")
+ endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed", duration=1)
+ endcondition.add_child(endcondition_part1)
+ endcondition.add_child(endcondition_part2)
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("Sequence Behavior")
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self._first_actor_transform))
+ sequence.add_child(ActorTransformSetter(self.other_actors[1], self._second_actor_transform))
+ sequence.add_child(driving_to_next_intersection)
+ sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
+ sequence.add_child(TimeOut(3))
+ sequence.add_child(obstacle_clear_road)
+ sequence.add_child(stop_near_intersection)
+ sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
+ sequence.add_child(endcondition)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+ sequence.add_child(ActorDestroy(self.other_actors[1]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/freeride.py b/scenario_runner/srunner/scenarios/freeride.py
new file mode 100644
index 0000000..b300f33
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/freeride.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Simple freeride scenario. No action, no triggers. Ego vehicle can simply cruise around.
+"""
+
+import py_trees
+
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class FreeRide(BasicScenario):
+
+ """
+ Implementation of a simple free ride scenario that consits only of the ego vehicle
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=10000000):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ super(FreeRide, self).__init__("FreeRide",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _setup_scenario_trigger(self, config):
+ """
+ """
+ return None
+
+ def _create_behavior(self):
+ """
+ """
+ sequence = py_trees.composites.Sequence("Sequence Behavior")
+ sequence.add_child(Idle())
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ for ego_vehicle in self.ego_vehicles:
+ collision_criterion = CollisionTest(ego_vehicle)
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/junction_crossing_route.py b/scenario_runner/srunner/scenarios/junction_crossing_route.py
new file mode 100644
index 0000000..af0ed3d
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/junction_crossing_route.py
@@ -0,0 +1,203 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+All intersection related scenarios that are part of a route.
+"""
+
+from __future__ import print_function
+
+import py_trees
+
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import TrafficLightManipulator
+
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, DrivenDistanceTest, MaxVelocityTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, WaitEndIntersection
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class SignalJunctionCrossingRoute(BasicScenario):
+
+ """
+ At routes, these scenarios are simplified, as they can be triggered making
+ use of the background activity. To ensure interactions with this background
+ activity, the traffic lights are modified, setting two of them to green
+ """
+
+ # ego vehicle parameters
+ _ego_max_velocity_allowed = 20 # Maximum allowed velocity [m/s]
+ _ego_expected_driven_distance = 50 # Expected driven distance [m]
+ _ego_distance_to_drive = 20 # Allowed distance to drive
+
+ _traffic_light = None
+
+ # Depending on the route, decide which traffic lights can be modified
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=180):
+ """
+ Setup all relevant parameters and create scenario
+ and instantiate scenario manager
+ """
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ self.subtype = config.subtype
+
+ super(SignalJunctionCrossingRoute, self).__init__("SignalJunctionCrossingRoute",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ def _create_behavior(self):
+ """
+ Scenario behavior:
+ When close to an intersection, the traffic lights will turn green for
+ both the ego_vehicle and another lane, allowing the background activity
+ to "run" their red light, creating scenarios 7, 8 and 9.
+
+ If this does not happen within 120 seconds, a timeout stops the scenario
+ """
+
+ # Changes traffic lights
+ traffic_hack = TrafficLightManipulator(self.ego_vehicles[0], self.subtype)
+
+ # finally wait that ego vehicle drove a specific distance
+ wait = DriveDistance(
+ self.ego_vehicles[0],
+ self._ego_distance_to_drive,
+ name="DriveDistance")
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("SignalJunctionCrossingRoute")
+ sequence.add_child(traffic_hack)
+ sequence.add_child(wait)
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ max_velocity_criterion = MaxVelocityTest(
+ self.ego_vehicles[0],
+ self._ego_max_velocity_allowed,
+ optional=True)
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ driven_distance_criterion = DrivenDistanceTest(
+ self.ego_vehicles[0],
+ self._ego_expected_driven_distance)
+
+ criteria.append(max_velocity_criterion)
+ criteria.append(collision_criterion)
+ criteria.append(driven_distance_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors and traffic lights upon deletion
+ """
+ self._traffic_light = None
+ self.remove_all_actors()
+
+
+class NoSignalJunctionCrossingRoute(BasicScenario):
+
+ """
+ At routes, these scenarios are simplified, as they can be triggered making
+ use of the background activity. For unsignalized intersections, just wait
+ until the ego_vehicle has left the intersection.
+ """
+
+ # ego vehicle parameters
+ _ego_max_velocity_allowed = 20 # Maximum allowed velocity [m/s]
+ _ego_expected_driven_distance = 50 # Expected driven distance [m]
+ _ego_distance_to_drive = 20 # Allowed distance to drive
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=180):
+ """
+ Setup all relevant parameters and create scenario
+ and instantiate scenario manager
+ """
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(NoSignalJunctionCrossingRoute, self).__init__("NoSignalJunctionCrossingRoute",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ def _create_behavior(self):
+ """
+ Scenario behavior:
+ When close to an intersection, the traffic lights will turn green for
+ both the ego_vehicle and another lane, allowing the background activity
+ to "run" their red light.
+
+ If this does not happen within 120 seconds, a timeout stops the scenario
+ """
+ # finally wait that ego vehicle drove a specific distance
+ wait = WaitEndIntersection(
+ self.ego_vehicles[0],
+ name="WaitEndIntersection")
+ end_condition = DriveDistance(
+ self.ego_vehicles[0],
+ self._ego_distance_to_drive,
+ name="DriveDistance")
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("NoSignalJunctionCrossingRoute")
+ sequence.add_child(wait)
+ sequence.add_child(end_condition)
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ max_velocity_criterion = MaxVelocityTest(
+ self.ego_vehicles[0],
+ self._ego_max_velocity_allowed,
+ optional=True)
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ driven_distance_criterion = DrivenDistanceTest(
+ self.ego_vehicles[0],
+ self._ego_expected_driven_distance)
+
+ criteria.append(max_velocity_criterion)
+ criteria.append(collision_criterion)
+ criteria.append(driven_distance_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors and traffic lights upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/maneuver_opposite_direction.py b/scenario_runner/srunner/scenarios/maneuver_opposite_direction.py
new file mode 100644
index 0000000..df20870
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/maneuver_opposite_direction.py
@@ -0,0 +1,172 @@
+#!/usr/bin/env python
+
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Vehicle Maneuvering In Opposite Direction:
+
+Vehicle is passing another vehicle in a rural area, in daylight, under clear
+weather conditions, at a non-junction and encroaches into another
+vehicle traveling in the opposite direction.
+"""
+
+from six.moves.queue import Queue # pylint: disable=relative-import
+
+import math
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ ActorSource,
+ ActorSink,
+ WaypointFollower)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_waypoint_in_distance
+
+
+class ManeuverOppositeDirection(BasicScenario):
+
+ """
+ "Vehicle Maneuvering In Opposite Direction" (Traffic Scenario 06)
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ obstacle_type='barrier', timeout=120):
+ """
+ Setup all relevant parameters and create scenario
+ obstacle_type -> flag to select type of leading obstacle. Values: vehicle, barrier
+ """
+ self._world = world
+ self._map = CarlaDataProvider.get_map()
+ self._first_vehicle_location = 50
+ self._second_vehicle_location = self._first_vehicle_location + 60
+ self._ego_vehicle_drive_distance = self._second_vehicle_location * 2
+ self._start_distance = self._first_vehicle_location * 0.9
+ self._opposite_speed = 5.56 # m/s
+ self._source_gap = 40 # m
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+ self._source_transform = None
+ self._sink_location = None
+ self._blackboard_queue_name = 'ManeuverOppositeDirection/actor_flow_queue'
+ self._queue = py_trees.blackboard.Blackboard().set(self._blackboard_queue_name, Queue())
+ self._obstacle_type = obstacle_type
+ self._first_actor_transform = None
+ self._second_actor_transform = None
+ self._third_actor_transform = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(ManeuverOppositeDirection, self).__init__(
+ "ManeuverOppositeDirection",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ first_actor_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._first_vehicle_location)
+ second_actor_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._second_vehicle_location)
+ second_actor_waypoint = second_actor_waypoint.get_left_lane()
+
+ first_actor_transform = carla.Transform(
+ first_actor_waypoint.transform.location,
+ first_actor_waypoint.transform.rotation)
+ if self._obstacle_type == 'vehicle':
+ first_actor_model = 'vehicle.nissan.micra'
+ else:
+ first_actor_transform.rotation.yaw += 90
+ first_actor_model = 'static.prop.streetbarrier'
+ second_prop_waypoint = first_actor_waypoint.next(2.0)[0]
+ position_yaw = second_prop_waypoint.transform.rotation.yaw + 90
+ offset_location = carla.Location(
+ 0.50 * second_prop_waypoint.lane_width * math.cos(math.radians(position_yaw)),
+ 0.50 * second_prop_waypoint.lane_width * math.sin(math.radians(position_yaw)))
+ second_prop_transform = carla.Transform(
+ second_prop_waypoint.transform.location + offset_location, first_actor_transform.rotation)
+ second_prop_actor = CarlaDataProvider.request_new_actor(first_actor_model, second_prop_transform)
+ second_prop_actor.set_simulate_physics(True)
+ first_actor = CarlaDataProvider.request_new_actor(first_actor_model, first_actor_transform)
+ first_actor.set_simulate_physics(True)
+ second_actor = CarlaDataProvider.request_new_actor('vehicle.audi.tt', second_actor_waypoint.transform)
+
+ self.other_actors.append(first_actor)
+ self.other_actors.append(second_actor)
+ if self._obstacle_type != 'vehicle':
+ self.other_actors.append(second_prop_actor)
+
+ self._source_transform = second_actor_waypoint.transform
+ sink_waypoint = second_actor_waypoint.next(1)[0]
+ while not sink_waypoint.is_intersection:
+ sink_waypoint = sink_waypoint.next(1)[0]
+ self._sink_location = sink_waypoint.transform.location
+
+ self._first_actor_transform = first_actor_transform
+ self._second_actor_transform = second_actor_waypoint.transform
+ self._third_actor_transform = second_prop_transform
+
+ def _create_behavior(self):
+ """
+ The behavior tree returned by this method is as follows:
+ The ego vehicle is trying to pass a leading vehicle in the same lane
+ by moving onto the oncoming lane while another vehicle is moving in the
+ opposite direction in the oncoming lane.
+ """
+
+ # Leaf nodes
+ actor_source = ActorSource(
+ ['vehicle.audi.tt', 'vehicle.tesla.model3', 'vehicle.nissan.micra'],
+ self._source_transform, self._source_gap, self._blackboard_queue_name)
+ actor_sink = ActorSink(self._sink_location, 10)
+ ego_drive_distance = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_drive_distance)
+ waypoint_follower = WaypointFollower(
+ self.other_actors[1], self._opposite_speed,
+ blackboard_queue_name=self._blackboard_queue_name, avoid_collision=True)
+
+ # Non-leaf nodes
+ parallel_root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ # Building tree
+ parallel_root.add_child(ego_drive_distance)
+ parallel_root.add_child(actor_source)
+ parallel_root.add_child(actor_sink)
+ parallel_root.add_child(waypoint_follower)
+
+ scenario_sequence = py_trees.composites.Sequence()
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._first_actor_transform))
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[1], self._second_actor_transform))
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[2], self._third_actor_transform))
+ scenario_sequence.add_child(parallel_root)
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[1]))
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[2]))
+
+ return scenario_sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/master_scenario.py b/scenario_runner/srunner/scenarios/master_scenario.py
new file mode 100644
index 0000000..87fc4ff
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/master_scenario.py
@@ -0,0 +1,114 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Basic CARLA Autonomous Driving training scenario
+"""
+
+import py_trees
+
+from srunner.scenarioconfigs.route_scenario_configuration import RouteConfiguration
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest,
+ InRouteTest,
+ RouteCompletionTest,
+ OutsideRouteLanesTest,
+ RunningRedLightTest,
+ RunningStopTest,
+ ActorSpeedAboveThresholdTest)
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class MasterScenario(BasicScenario):
+
+ """
+ Implementation of a Master scenario that controls the route.
+
+ This is a single ego vehicle scenario
+ """
+
+ radius = 10.0 # meters
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=300):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self.config = config
+ self.route = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ if hasattr(self.config, 'route'):
+ self.route = self.config.route
+ else:
+ raise ValueError("Master scenario must have a route")
+
+ super(MasterScenario, self).__init__("MasterScenario", ego_vehicles=ego_vehicles, config=config,
+ world=world, debug_mode=debug_mode,
+ terminate_on_failure=True, criteria_enable=criteria_enable)
+
+ def _create_behavior(self):
+ """
+ Basic behavior do nothing, i.e. Idle
+ """
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("MasterScenario")
+ idle_behavior = Idle()
+ sequence.add_child(idle_behavior)
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+
+ if isinstance(self.route, RouteConfiguration):
+ route = self.route.data
+ else:
+ route = self.route
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0], terminate_on_failure=False)
+
+ route_criterion = InRouteTest(self.ego_vehicles[0],
+ route=route,
+ offroad_max=30,
+ terminate_on_failure=True)
+
+ completion_criterion = RouteCompletionTest(self.ego_vehicles[0], route=route)
+
+ outsidelane_criterion = OutsideRouteLanesTest(self.ego_vehicles[0], route=route)
+
+ red_light_criterion = RunningRedLightTest(self.ego_vehicles[0])
+
+ stop_criterion = RunningStopTest(self.ego_vehicles[0])
+
+ blocked_criterion = ActorSpeedAboveThresholdTest(self.ego_vehicles[0],
+ speed_threshold=0.1,
+ below_threshold_max_time=90.0,
+ terminate_on_failure=True)
+
+ parallel_criteria = py_trees.composites.Parallel("group_criteria",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ parallel_criteria.add_child(completion_criterion)
+ parallel_criteria.add_child(collision_criterion)
+ parallel_criteria.add_child(route_criterion)
+ parallel_criteria.add_child(outsidelane_criterion)
+ parallel_criteria.add_child(red_light_criterion)
+ parallel_criteria.add_child(stop_criterion)
+ parallel_criteria.add_child(blocked_criterion)
+
+ return parallel_criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/no_signal_junction_crossing.py b/scenario_runner/srunner/scenarios/no_signal_junction_crossing.py
new file mode 100644
index 0000000..0c24965
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/no_signal_junction_crossing.py
@@ -0,0 +1,164 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Non-signalized junctions: crossing negotiation:
+
+The hero vehicle is passing through a junction without traffic lights
+And encounters another vehicle passing across the junction.
+"""
+
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ SyncArrival,
+ KeepVelocity,
+ StopVehicle)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import InTriggerRegion
+from srunner.scenarios.basic_scenario import BasicScenario
+
+
+class NoSignalJunctionCrossing(BasicScenario):
+
+ """
+ Implementation class for
+ 'Non-signalized junctions: crossing negotiation' scenario,
+ (Traffic Scenario 10).
+
+ This is a single ego vehicle scenario
+ """
+
+ # ego vehicle parameters
+ _ego_vehicle_max_velocity = 20
+ _ego_vehicle_driven_distance = 105
+
+ # other vehicle
+ _other_actor_max_brake = 1.0
+ _other_actor_target_velocity = 15
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+
+ self._other_actor_transform = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(NoSignalJunctionCrossing, self).__init__("NoSignalJunctionCrossing",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ self._other_actor_transform = config.other_actors[0].transform
+ first_vehicle_transform = carla.Transform(
+ carla.Location(config.other_actors[0].transform.location.x,
+ config.other_actors[0].transform.location.y,
+ config.other_actors[0].transform.location.z - 500),
+ config.other_actors[0].transform.rotation)
+ first_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, first_vehicle_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ After invoking this scenario, it will wait for the user
+ controlled vehicle to enter the start region,
+ then make a traffic participant to accelerate
+ until it is going fast enough to reach an intersection point.
+ at the same time as the user controlled vehicle at the junction.
+ Once the user controlled vehicle comes close to the junction,
+ the traffic participant accelerates and passes through the junction.
+ After 60 seconds, a timeout stops the scenario.
+ """
+
+ # Creating leaf nodes
+ start_other_trigger = InTriggerRegion(
+ self.ego_vehicles[0],
+ -80, -70,
+ -75, -60)
+
+ sync_arrival = SyncArrival(
+ self.other_actors[0], self.ego_vehicles[0],
+ carla.Location(x=-74.63, y=-136.34))
+
+ pass_through_trigger = InTriggerRegion(
+ self.ego_vehicles[0],
+ -90, -70,
+ -124, -119)
+
+ keep_velocity_other = KeepVelocity(
+ self.other_actors[0],
+ self._other_actor_target_velocity)
+
+ stop_other_trigger = InTriggerRegion(
+ self.other_actors[0],
+ -45, -35,
+ -140, -130)
+
+ stop_other = StopVehicle(
+ self.other_actors[0],
+ self._other_actor_max_brake)
+
+ end_condition = InTriggerRegion(
+ self.ego_vehicles[0],
+ -90, -70,
+ -170, -156
+ )
+
+ # Creating non-leaf nodes
+ root = py_trees.composites.Sequence()
+ scenario_sequence = py_trees.composites.Sequence()
+ sync_arrival_parallel = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ keep_velocity_other_parallel = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ # Building tree
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform))
+ scenario_sequence.add_child(start_other_trigger)
+ scenario_sequence.add_child(sync_arrival_parallel)
+ scenario_sequence.add_child(keep_velocity_other_parallel)
+ scenario_sequence.add_child(stop_other)
+ scenario_sequence.add_child(end_condition)
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ sync_arrival_parallel.add_child(sync_arrival)
+ sync_arrival_parallel.add_child(pass_through_trigger)
+ keep_velocity_other_parallel.add_child(keep_velocity_other)
+ keep_velocity_other_parallel.add_child(stop_other_trigger)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collison_criteria = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collison_criteria)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/object_crash_intersection.py b/scenario_runner/srunner/scenarios/object_crash_intersection.py
new file mode 100644
index 0000000..a1c3037
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/object_crash_intersection.py
@@ -0,0 +1,606 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+"""
+Object crash with prior vehicle action scenario:
+The scenario realizes the user controlled ego vehicle
+moving along the road and encounters a cyclist ahead after taking a right or left turn.
+"""
+
+from __future__ import print_function
+
+import math
+import py_trees
+
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ KeepVelocity,
+ HandBrakeVehicle)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocationAlongRoute,
+ InTriggerDistanceToVehicle,
+ DriveDistance)
+from srunner.scenariomanager.timer import TimeOut
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import generate_target_waypoint, generate_target_waypoint_in_route
+
+
+def get_opponent_transform(added_dist, waypoint, trigger_location):
+ """
+ Calculate the transform of the adversary
+ """
+ lane_width = waypoint.lane_width
+
+ offset = {"orientation": 270, "position": 90, "k": 1.0}
+ _wp = waypoint.next(added_dist)
+ if _wp:
+ _wp = _wp[-1]
+ else:
+ raise RuntimeError("Cannot get next waypoint !")
+
+ location = _wp.transform.location
+ orientation_yaw = _wp.transform.rotation.yaw + offset["orientation"]
+ position_yaw = _wp.transform.rotation.yaw + offset["position"]
+
+ offset_location = carla.Location(
+ offset['k'] * lane_width * math.cos(math.radians(position_yaw)),
+ offset['k'] * lane_width * math.sin(math.radians(position_yaw)))
+ location += offset_location
+ location.z = trigger_location.z
+ transform = carla.Transform(location, carla.Rotation(yaw=orientation_yaw))
+
+ return transform
+
+
+def get_right_driving_lane(waypoint):
+ """
+ Gets the driving / parking lane that is most to the right of the waypoint
+ as well as the number of lane changes done
+ """
+ lane_changes = 0
+
+ while True:
+ wp_next = waypoint.get_right_lane()
+ lane_changes += 1
+
+ if wp_next is None or wp_next.lane_type == carla.LaneType.Sidewalk:
+ break
+ elif wp_next.lane_type == carla.LaneType.Shoulder:
+ # Filter Parkings considered as Shoulders
+ if is_lane_a_parking(wp_next):
+ lane_changes += 1
+ waypoint = wp_next
+ break
+ else:
+ waypoint = wp_next
+
+ return waypoint, lane_changes
+
+
+def is_lane_a_parking(waypoint):
+ """
+ This function filters false negative Shoulder which are in reality Parking lanes.
+ These are differentiated from the others because, similar to the driving lanes,
+ they have, on the right, a small Shoulder followed by a Sidewalk.
+ """
+
+ # Parking are wide lanes
+ if waypoint.lane_width > 2:
+ wp_next = waypoint.get_right_lane()
+
+ # That are next to a mini-Shoulder
+ if wp_next is not None and wp_next.lane_type == carla.LaneType.Shoulder:
+ wp_next_next = wp_next.get_right_lane()
+
+ # Followed by a Sidewalk
+ if wp_next_next is not None and wp_next_next.lane_type == carla.LaneType.Sidewalk:
+ return True
+
+ return False
+
+
+class VehicleTurningRight(BasicScenario):
+
+ """
+ This class holds everything required for a simple object crash
+ with prior vehicle action involving a vehicle and a cyclist.
+ The ego vehicle is passing through a road and encounters
+ a cyclist after taking a right turn. (Traffic Scenario 4)
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+
+ self._other_actor_target_velocity = 10
+ self._wmap = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._wmap.get_waypoint(config.trigger_points[0].location)
+ self._trigger_location = config.trigger_points[0].location
+ self._other_actor_transform = None
+ self._num_lane_changes = 0
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ # Total Number of attempts to relocate a vehicle before spawning
+ self._number_of_attempts = 6
+ # Number of attempts made so far
+ self._spawn_attempted = 0
+
+ self._ego_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ super(VehicleTurningRight, self).__init__("VehicleTurningRight",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ # Get the waypoint right after the junction
+ waypoint = generate_target_waypoint(self._reference_waypoint, 1)
+
+ # Move a certain distance to the front
+ start_distance = 8
+ waypoint = waypoint.next(start_distance)[0]
+
+ # Get the last driving lane to the right
+ waypoint, self._num_lane_changes = get_right_driving_lane(waypoint)
+ # And for synchrony purposes, move to the front a bit
+ added_dist = self._num_lane_changes
+
+ while True:
+
+ # Try to spawn the actor
+ try:
+ self._other_actor_transform = get_opponent_transform(added_dist, waypoint, self._trigger_location)
+ first_vehicle = CarlaDataProvider.request_new_actor(
+ 'vehicle.diamondback.century', self._other_actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ break
+
+ # Move the spawning point a bit and try again
+ except RuntimeError as r:
+ # In the case there is an object just move a little bit and retry
+ print(" Base transform is blocking objects ", self._other_actor_transform)
+ added_dist += 0.5
+ self._spawn_attempted += 1
+ if self._spawn_attempted >= self._number_of_attempts:
+ raise r
+
+ # Set the transform to -500 z after we are able to spawn it
+ actor_transform = carla.Transform(
+ carla.Location(self._other_actor_transform.location.x,
+ self._other_actor_transform.location.y,
+ self._other_actor_transform.location.z - 500),
+ self._other_actor_transform.rotation)
+ first_vehicle.set_transform(actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ After invoking this scenario, cyclist will wait for the user
+ controlled vehicle to enter the in the trigger distance region,
+ the cyclist starts crossing the road once the condition meets,
+ ego vehicle has to avoid the crash after a right turn, but
+ continue driving after the road is clear.If this does not happen
+ within 90 seconds, a timeout stops the scenario.
+ """
+
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="IntersectionRightTurn")
+
+ lane_width = self._reference_waypoint.lane_width
+ dist_to_travel = lane_width + (1.10 * lane_width * self._num_lane_changes)
+
+ bycicle_start_dist = 13 + dist_to_travel
+
+ if self._ego_route is not None:
+ trigger_distance = InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
+ self._ego_route,
+ self._other_actor_transform.location,
+ bycicle_start_dist)
+ else:
+ trigger_distance = InTriggerDistanceToVehicle(self.other_actors[0],
+ self.ego_vehicles[0],
+ bycicle_start_dist)
+
+ actor_velocity = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ actor_traverse = DriveDistance(self.other_actors[0], 0.30 * dist_to_travel)
+ post_timer_velocity_actor = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ post_timer_traverse_actor = DriveDistance(self.other_actors[0], 0.70 * dist_to_travel)
+ end_condition = TimeOut(5)
+
+ # non leaf nodes
+ scenario_sequence = py_trees.composites.Sequence()
+
+ actor_ego_sync = py_trees.composites.Parallel(
+ "Synchronization of actor and ego vehicle",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ after_timer_actor = py_trees.composites.Parallel(
+ "After timeout actor will cross the remaining lane_width",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ # building the tree
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform,
+ name='TransformSetterTS4'))
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], True))
+ scenario_sequence.add_child(trigger_distance)
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], False))
+ scenario_sequence.add_child(actor_ego_sync)
+ scenario_sequence.add_child(after_timer_actor)
+ scenario_sequence.add_child(end_condition)
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ actor_ego_sync.add_child(actor_velocity)
+ actor_ego_sync.add_child(actor_traverse)
+
+ after_timer_actor.add_child(post_timer_velocity_actor)
+ after_timer_actor.add_child(post_timer_traverse_actor)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
+
+
+class VehicleTurningLeft(BasicScenario):
+
+ """
+ This class holds everything required for a simple object crash
+ with prior vehicle action involving a vehicle and a cyclist.
+ The ego vehicle is passing through a road and encounters
+ a cyclist after taking a left turn. (Traffic Scenario 4)
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+
+ self._other_actor_target_velocity = 10
+ self._wmap = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._wmap.get_waypoint(config.trigger_points[0].location)
+ self._trigger_location = config.trigger_points[0].location
+ self._other_actor_transform = None
+ self._num_lane_changes = 0
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ # Total Number of attempts to relocate a vehicle before spawning
+ self._number_of_attempts = 6
+ # Number of attempts made so far
+ self._spawn_attempted = 0
+
+ self._ego_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ super(VehicleTurningLeft, self).__init__("VehicleTurningLeft",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ # Get the waypoint right after the junction
+ waypoint = generate_target_waypoint(self._reference_waypoint, -1)
+
+ # Move a certain distance to the front
+ start_distance = 8
+ waypoint = waypoint.next(start_distance)[0]
+
+ # Get the last driving lane to the right
+ waypoint, self._num_lane_changes = get_right_driving_lane(waypoint)
+ # And for synchrony purposes, move to the front a bit
+ added_dist = self._num_lane_changes
+
+ while True:
+
+ # Try to spawn the actor
+ try:
+ self._other_actor_transform = get_opponent_transform(added_dist, waypoint, self._trigger_location)
+ first_vehicle = CarlaDataProvider.request_new_actor(
+ 'vehicle.diamondback.century', self._other_actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ break
+
+ # Move the spawning point a bit and try again
+ except RuntimeError as r:
+ # In the case there is an object just move a little bit and retry
+ print(" Base transform is blocking objects ", self._other_actor_transform)
+ added_dist += 0.5
+ self._spawn_attempted += 1
+ if self._spawn_attempted >= self._number_of_attempts:
+ raise r
+
+ # Set the transform to -500 z after we are able to spawn it
+ actor_transform = carla.Transform(
+ carla.Location(self._other_actor_transform.location.x,
+ self._other_actor_transform.location.y,
+ self._other_actor_transform.location.z - 500),
+ self._other_actor_transform.rotation)
+ first_vehicle.set_transform(actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ After invoking this scenario, cyclist will wait for the user
+ controlled vehicle to enter the in the trigger distance region,
+ the cyclist starts crossing the road once the condition meets,
+ ego vehicle has to avoid the crash after a left turn, but
+ continue driving after the road is clear.If this does not happen
+ within 90 seconds, a timeout stops the scenario.
+ """
+
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="IntersectionLeftTurn")
+
+ lane_width = self._reference_waypoint.lane_width
+ dist_to_travel = lane_width + (1.10 * lane_width * self._num_lane_changes)
+
+ bycicle_start_dist = 13 + dist_to_travel
+
+ if self._ego_route is not None:
+ trigger_distance = InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
+ self._ego_route,
+ self._other_actor_transform.location,
+ bycicle_start_dist)
+ else:
+ trigger_distance = InTriggerDistanceToVehicle(self.other_actors[0],
+ self.ego_vehicles[0],
+ bycicle_start_dist)
+
+ actor_velocity = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ actor_traverse = DriveDistance(self.other_actors[0], 0.30 * dist_to_travel)
+ post_timer_velocity_actor = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ post_timer_traverse_actor = DriveDistance(self.other_actors[0], 0.70 * dist_to_travel)
+ end_condition = TimeOut(5)
+
+ # non leaf nodes
+ scenario_sequence = py_trees.composites.Sequence()
+
+ actor_ego_sync = py_trees.composites.Parallel(
+ "Synchronization of actor and ego vehicle",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ after_timer_actor = py_trees.composites.Parallel(
+ "After timeout actor will cross the remaining lane_width",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ # building the tree
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform,
+ name='TransformSetterTS4'))
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], True))
+ scenario_sequence.add_child(trigger_distance)
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], False))
+ scenario_sequence.add_child(actor_ego_sync)
+ scenario_sequence.add_child(after_timer_actor)
+ scenario_sequence.add_child(end_condition)
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ actor_ego_sync.add_child(actor_velocity)
+ actor_ego_sync.add_child(actor_traverse)
+
+ after_timer_actor.add_child(post_timer_velocity_actor)
+ after_timer_actor.add_child(post_timer_traverse_actor)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
+
+
+class VehicleTurningRoute(BasicScenario):
+
+ """
+ This class holds everything required for a simple object crash
+ with prior vehicle action involving a vehicle and a cyclist.
+ The ego vehicle is passing through a road and encounters
+ a cyclist after taking a turn. This is the version used when the ego vehicle
+ is following a given route. (Traffic Scenario 4)
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+
+ self._other_actor_target_velocity = 10
+ self._wmap = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._wmap.get_waypoint(config.trigger_points[0].location)
+ self._trigger_location = config.trigger_points[0].location
+ self._other_actor_transform = None
+ self._num_lane_changes = 0
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ # Total Number of attempts to relocate a vehicle before spawning
+ self._number_of_attempts = 6
+ # Number of attempts made so far
+ self._spawn_attempted = 0
+
+ self._ego_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ super(VehicleTurningRoute, self).__init__("VehicleTurningRoute",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+
+ # Get the waypoint right after the junction
+ waypoint = generate_target_waypoint_in_route(self._reference_waypoint, self._ego_route)
+
+ # Move a certain distance to the front
+ start_distance = 8
+ waypoint = waypoint.next(start_distance)[0]
+
+ # Get the last driving lane to the right
+ waypoint, self._num_lane_changes = get_right_driving_lane(waypoint)
+ # And for synchrony purposes, move to the front a bit
+ added_dist = self._num_lane_changes
+
+ while True:
+
+ # Try to spawn the actor
+ try:
+ self._other_actor_transform = get_opponent_transform(added_dist, waypoint, self._trigger_location)
+ first_vehicle = CarlaDataProvider.request_new_actor(
+ 'vehicle.diamondback.century', self._other_actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ break
+
+ # Move the spawning point a bit and try again
+ except RuntimeError as r:
+ # In the case there is an object just move a little bit and retry
+ print(" Base transform is blocking objects ", self._other_actor_transform)
+ added_dist += 0.5
+ self._spawn_attempted += 1
+ if self._spawn_attempted >= self._number_of_attempts:
+ raise r
+
+ # Set the transform to -500 z after we are able to spawn it
+ actor_transform = carla.Transform(
+ carla.Location(self._other_actor_transform.location.x,
+ self._other_actor_transform.location.y,
+ self._other_actor_transform.location.z - 500),
+ self._other_actor_transform.rotation)
+ first_vehicle.set_transform(actor_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ After invoking this scenario, cyclist will wait for the user
+ controlled vehicle to enter the in the trigger distance region,
+ the cyclist starts crossing the road once the condition meets,
+ ego vehicle has to avoid the crash after a turn, but
+ continue driving after the road is clear.If this does not happen
+ within 90 seconds, a timeout stops the scenario.
+ """
+
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="IntersectionRouteTurn")
+
+ lane_width = self._reference_waypoint.lane_width
+ dist_to_travel = lane_width + (1.10 * lane_width * self._num_lane_changes)
+
+ bycicle_start_dist = 13 + dist_to_travel
+
+ if self._ego_route is not None:
+ trigger_distance = InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
+ self._ego_route,
+ self._other_actor_transform.location,
+ bycicle_start_dist)
+ else:
+ trigger_distance = InTriggerDistanceToVehicle(self.other_actors[0],
+ self.ego_vehicles[0],
+ bycicle_start_dist)
+
+ actor_velocity = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ actor_traverse = DriveDistance(self.other_actors[0], 0.30 * dist_to_travel)
+ post_timer_velocity_actor = KeepVelocity(self.other_actors[0], self._other_actor_target_velocity)
+ post_timer_traverse_actor = DriveDistance(self.other_actors[0], 0.70 * dist_to_travel)
+ end_condition = TimeOut(5)
+
+ # non leaf nodes
+ scenario_sequence = py_trees.composites.Sequence()
+
+ actor_ego_sync = py_trees.composites.Parallel(
+ "Synchronization of actor and ego vehicle",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ after_timer_actor = py_trees.composites.Parallel(
+ "After timeout actor will cross the remaining lane_width",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ # building the tree
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform,
+ name='TransformSetterTS4'))
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], True))
+ scenario_sequence.add_child(trigger_distance)
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], False))
+ scenario_sequence.add_child(actor_ego_sync)
+ scenario_sequence.add_child(after_timer_actor)
+ scenario_sequence.add_child(end_condition)
+ scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ actor_ego_sync.add_child(actor_velocity)
+ actor_ego_sync.add_child(actor_traverse)
+
+ after_timer_actor.add_child(post_timer_velocity_actor)
+ after_timer_actor.add_child(post_timer_traverse_actor)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+
+ criteria.append(collision_criterion)
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/object_crash_vehicle.py b/scenario_runner/srunner/scenarios/object_crash_vehicle.py
new file mode 100644
index 0000000..e540e97
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/object_crash_vehicle.py
@@ -0,0 +1,404 @@
+#!/usr/bin/env python
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Object crash without prior vehicle action scenario:
+The scenario realizes the user controlled ego vehicle
+moving along the road and encountering a cyclist ahead.
+"""
+
+from __future__ import print_function
+
+import math
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ AccelerateToVelocity,
+ HandBrakeVehicle,
+ KeepVelocity,
+ StopVehicle)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocationAlongRoute,
+ InTimeToArrivalToVehicle,
+ DriveDistance)
+from srunner.scenariomanager.timer import TimeOut
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_location_in_distance_from_wp
+
+
+class StationaryObjectCrossing(BasicScenario):
+
+ """
+ This class holds everything required for a simple object crash
+ without prior vehicle action involving a vehicle and a cyclist.
+ The ego vehicle is passing through a road and encounters
+ a stationary cyclist.
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._wmap = CarlaDataProvider.get_map()
+ self._reference_waypoint = self._wmap.get_waypoint(config.trigger_points[0].location)
+ # ego vehicle parameters
+ self._ego_vehicle_distance_driven = 40
+
+ # other vehicle parameters
+ self._other_actor_target_velocity = 10
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(StationaryObjectCrossing, self).__init__("Stationaryobjectcrossing",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ _start_distance = 40
+ lane_width = self._reference_waypoint.lane_width
+ location, _ = get_location_in_distance_from_wp(self._reference_waypoint, _start_distance)
+ waypoint = self._wmap.get_waypoint(location)
+ offset = {"orientation": 270, "position": 90, "z": 0.4, "k": 0.2}
+ position_yaw = waypoint.transform.rotation.yaw + offset['position']
+ orientation_yaw = waypoint.transform.rotation.yaw + offset['orientation']
+ offset_location = carla.Location(
+ offset['k'] * lane_width * math.cos(math.radians(position_yaw)),
+ offset['k'] * lane_width * math.sin(math.radians(position_yaw)))
+ location += offset_location
+ location.z += offset['z']
+ self.transform = carla.Transform(location, carla.Rotation(yaw=orientation_yaw))
+ static = CarlaDataProvider.request_new_actor('static.prop.container', self.transform)
+ static.set_simulate_physics(True)
+ self.other_actors.append(static)
+
+ def _create_behavior(self):
+ """
+ Only behavior here is to wait
+ """
+ lane_width = self.ego_vehicles[0].get_world().get_map().get_waypoint(
+ self.ego_vehicles[0].get_location()).lane_width
+ lane_width = lane_width + (1.25 * lane_width)
+
+ # leaf nodes
+ actor_stand = TimeOut(15)
+ actor_removed = ActorDestroy(self.other_actors[0])
+ end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven)
+
+ # non leaf nodes
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ scenario_sequence = py_trees.composites.Sequence()
+
+ # building tree
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self.transform))
+ scenario_sequence.add_child(actor_stand)
+ scenario_sequence.add_child(actor_removed)
+ scenario_sequence.add_child(end_condition)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
+
+
+class DynamicObjectCrossing(BasicScenario):
+
+ """
+ This class holds everything required for a simple object crash
+ without prior vehicle action involving a vehicle and a cyclist/pedestrian,
+ The ego vehicle is passing through a road,
+ And encounters a cyclist/pedestrian crossing the road.
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False,
+ debug_mode=False, criteria_enable=True, adversary_type=False, timeout=60):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._wmap = CarlaDataProvider.get_map()
+
+ self._reference_waypoint = self._wmap.get_waypoint(config.trigger_points[0].location)
+ # ego vehicle parameters
+ self._ego_vehicle_distance_driven = 40
+ # other vehicle parameters
+ self._other_actor_target_velocity = 5
+ self._other_actor_max_brake = 1.0
+ self._time_to_reach = 10
+ self._adversary_type = adversary_type # flag to select either pedestrian (False) or cyclist (True)
+ self._walker_yaw = 0
+ self._num_lane_changes = 1
+ self.transform = None
+ self.transform2 = None
+ self.timeout = timeout
+ self._trigger_location = config.trigger_points[0].location
+ # Total Number of attempts to relocate a vehicle before spawning
+ self._number_of_attempts = 20
+ # Number of attempts made so far
+ self._spawn_attempted = 0
+
+ self._ego_route = CarlaDataProvider.get_ego_vehicle_route()
+
+ super(DynamicObjectCrossing, self).__init__("DynamicObjectCrossing",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _calculate_base_transform(self, _start_distance, waypoint):
+
+ lane_width = waypoint.lane_width
+
+ # Patches false junctions
+ if self._reference_waypoint.is_junction:
+ stop_at_junction = False
+ else:
+ stop_at_junction = True
+
+ location, _ = get_location_in_distance_from_wp(waypoint, _start_distance, stop_at_junction)
+ waypoint = self._wmap.get_waypoint(location)
+ offset = {"orientation": 270, "position": 90, "z": 0.6, "k": 1.0}
+ position_yaw = waypoint.transform.rotation.yaw + offset['position']
+ orientation_yaw = waypoint.transform.rotation.yaw + offset['orientation']
+ offset_location = carla.Location(
+ offset['k'] * lane_width * math.cos(math.radians(position_yaw)),
+ offset['k'] * lane_width * math.sin(math.radians(position_yaw)))
+ location += offset_location
+ location.z = self._trigger_location.z + offset['z']
+ return carla.Transform(location, carla.Rotation(yaw=orientation_yaw)), orientation_yaw
+
+ def _spawn_adversary(self, transform, orientation_yaw):
+
+ self._time_to_reach *= self._num_lane_changes
+
+ if self._adversary_type is False:
+ self._walker_yaw = orientation_yaw
+ self._other_actor_target_velocity = 3 + (0.4 * self._num_lane_changes)
+ walker = CarlaDataProvider.request_new_actor('walker.*', transform)
+ adversary = walker
+ else:
+ self._other_actor_target_velocity = self._other_actor_target_velocity * self._num_lane_changes
+ first_vehicle = CarlaDataProvider.request_new_actor('vehicle.diamondback.century', transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ adversary = first_vehicle
+
+ return adversary
+
+ def _spawn_blocker(self, transform, orientation_yaw):
+ """
+ Spawn the blocker prop that blocks the vision from the egovehicle of the jaywalker
+ :return:
+ """
+ # static object transform
+ shift = 0.9
+ x_ego = self._reference_waypoint.transform.location.x
+ y_ego = self._reference_waypoint.transform.location.y
+ x_cycle = transform.location.x
+ y_cycle = transform.location.y
+ x_static = x_ego + shift * (x_cycle - x_ego)
+ y_static = y_ego + shift * (y_cycle - y_ego)
+
+ spawn_point_wp = self.ego_vehicles[0].get_world().get_map().get_waypoint(transform.location)
+
+ self.transform2 = carla.Transform(carla.Location(x_static, y_static,
+ spawn_point_wp.transform.location.z + 0.3),
+ carla.Rotation(yaw=orientation_yaw + 180))
+
+ static = CarlaDataProvider.request_new_actor('static.prop.vendingmachine', self.transform2)
+ static.set_simulate_physics(enabled=False)
+
+ return static
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ # cyclist transform
+ _start_distance = 12
+ # We start by getting and waypoint in the closest sidewalk.
+ waypoint = self._reference_waypoint
+ while True:
+ wp_next = waypoint.get_right_lane()
+ self._num_lane_changes += 1
+ if wp_next is None or wp_next.lane_type == carla.LaneType.Sidewalk:
+ break
+ elif wp_next.lane_type == carla.LaneType.Shoulder:
+ # Filter Parkings considered as Shoulders
+ if wp_next.lane_width > 2:
+ _start_distance += 1.5
+ waypoint = wp_next
+ break
+ else:
+ _start_distance += 1.5
+ waypoint = wp_next
+
+ while True: # We keep trying to spawn avoiding props
+
+ try:
+ self.transform, orientation_yaw = self._calculate_base_transform(_start_distance, waypoint)
+ first_vehicle = self._spawn_adversary(self.transform, orientation_yaw)
+
+ blocker = self._spawn_blocker(self.transform, orientation_yaw)
+
+ break
+ except RuntimeError as r:
+ # We keep retrying until we spawn
+ print("Base transform is blocking objects ", self.transform)
+ _start_distance += 0.4
+ self._spawn_attempted += 1
+ if self._spawn_attempted >= self._number_of_attempts:
+ raise r
+
+ # Now that we found a possible position we just put the vehicle to the underground
+ disp_transform = carla.Transform(
+ carla.Location(self.transform.location.x,
+ self.transform.location.y,
+ self.transform.location.z - 500),
+ self.transform.rotation)
+
+ prop_disp_transform = carla.Transform(
+ carla.Location(self.transform2.location.x,
+ self.transform2.location.y,
+ self.transform2.location.z - 500),
+ self.transform2.rotation)
+
+ first_vehicle.set_transform(disp_transform)
+ blocker.set_transform(prop_disp_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ blocker.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+ self.other_actors.append(blocker)
+
+ def _create_behavior(self):
+ """
+ After invoking this scenario, cyclist will wait for the user
+ controlled vehicle to enter trigger distance region,
+ the cyclist starts crossing the road once the condition meets,
+ then after 60 seconds, a timeout stops the scenario
+ """
+
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="OccludedObjectCrossing")
+ lane_width = self._reference_waypoint.lane_width
+ lane_width = lane_width + (1.25 * lane_width * self._num_lane_changes)
+
+ dist_to_trigger = 12 + self._num_lane_changes
+ # leaf nodes
+ if self._ego_route is not None:
+ start_condition = InTriggerDistanceToLocationAlongRoute(self.ego_vehicles[0],
+ self._ego_route,
+ self.transform.location,
+ dist_to_trigger)
+ else:
+ start_condition = InTimeToArrivalToVehicle(self.ego_vehicles[0],
+ self.other_actors[0],
+ self._time_to_reach)
+
+ actor_velocity = KeepVelocity(self.other_actors[0],
+ self._other_actor_target_velocity,
+ name="walker velocity")
+ actor_drive = DriveDistance(self.other_actors[0],
+ 0.5 * lane_width,
+ name="walker drive distance")
+ actor_start_cross_lane = AccelerateToVelocity(self.other_actors[0],
+ 1.0,
+ self._other_actor_target_velocity,
+ name="walker crossing lane accelerate velocity")
+ actor_cross_lane = DriveDistance(self.other_actors[0],
+ lane_width,
+ name="walker drive distance for lane crossing ")
+ actor_stop_crossed_lane = StopVehicle(self.other_actors[0],
+ self._other_actor_max_brake,
+ name="walker stop")
+ ego_pass_machine = DriveDistance(self.ego_vehicles[0],
+ 5,
+ name="ego vehicle passed prop")
+ actor_remove = ActorDestroy(self.other_actors[0],
+ name="Destroying walker")
+ static_remove = ActorDestroy(self.other_actors[1],
+ name="Destroying Prop")
+ end_condition = DriveDistance(self.ego_vehicles[0],
+ self._ego_vehicle_distance_driven,
+ name="End condition ego drive distance")
+
+ # non leaf nodes
+
+ scenario_sequence = py_trees.composites.Sequence()
+ keep_velocity_other = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="keep velocity other")
+ keep_velocity = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="keep velocity")
+
+ # building tree
+
+ root.add_child(scenario_sequence)
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[0], self.transform,
+ name='TransformSetterTS3walker'))
+ scenario_sequence.add_child(ActorTransformSetter(self.other_actors[1], self.transform2,
+ name='TransformSetterTS3coca', physics=False))
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], True))
+ scenario_sequence.add_child(start_condition)
+ scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[0], False))
+ scenario_sequence.add_child(keep_velocity)
+ scenario_sequence.add_child(keep_velocity_other)
+ scenario_sequence.add_child(actor_stop_crossed_lane)
+ scenario_sequence.add_child(actor_remove)
+ scenario_sequence.add_child(static_remove)
+ scenario_sequence.add_child(end_condition)
+
+ keep_velocity.add_child(actor_velocity)
+ keep_velocity.add_child(actor_drive)
+ keep_velocity_other.add_child(actor_start_cross_lane)
+ keep_velocity_other.add_child(actor_cross_lane)
+ keep_velocity_other.add_child(ego_pass_machine)
+
+ return root
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/open_scenario.py b/scenario_runner/srunner/scenarios/open_scenario.py
new file mode 100644
index 0000000..f50d6ce
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/open_scenario.py
@@ -0,0 +1,455 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Basic scenario class using the OpenSCENARIO definition
+"""
+
+from __future__ import print_function
+
+import itertools
+import py_trees
+
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeWeather, ChangeRoadFriction
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeActorControl, ChangeActorTargetSpeed
+from srunner.scenariomanager.timer import GameTime
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.openscenario_parser import OpenScenarioParser
+from srunner.tools.py_trees_port import Decorator, oneshot_behavior
+
+
+def repeatable_behavior(behaviour, name=None):
+ """
+ This behaviour allows a composite with oneshot ancestors to run multiple
+ times, resetting the oneshot variables after each execution
+ """
+ if not name:
+ name = behaviour.name
+ clear_descendant_variables = ClearBlackboardVariablesStartingWith(
+ name="Clear Descendant Variables of {}".format(name),
+ variable_name_beginning=name + ">"
+ )
+ # If it's a sequence, don't double-nest it in a redundant manner
+ if isinstance(behaviour, py_trees.composites.Sequence):
+ behaviour.add_child(clear_descendant_variables)
+ sequence = behaviour
+ else:
+ sequence = py_trees.composites.Sequence(name="RepeatableBehaviour of {}".format(name))
+ sequence.add_children([behaviour, clear_descendant_variables])
+ return sequence
+
+
+class ClearBlackboardVariablesStartingWith(py_trees.behaviours.Success):
+
+ """
+ Clear the values starting with the specified string from the blackboard.
+
+ Args:
+ name (:obj:`str`): name of the behaviour
+ variable_name_beginning (:obj:`str`): beginning of the names of variable to clear
+ """
+
+ def __init__(self,
+ name="Clear Blackboard Variable Starting With",
+ variable_name_beginning="dummy",
+ ):
+ super(ClearBlackboardVariablesStartingWith, self).__init__(name)
+ self.variable_name_beginning = variable_name_beginning
+
+ def initialise(self):
+ """
+ Delete the variables from the blackboard.
+ """
+ blackboard_variables = [key for key, _ in py_trees.blackboard.Blackboard().__dict__.items(
+ ) if key.startswith(self.variable_name_beginning)]
+ for variable in blackboard_variables:
+ delattr(py_trees.blackboard.Blackboard(), variable)
+
+
+class StoryElementStatusToBlackboard(Decorator):
+
+ """
+ Reflect the status of the decorator's child story element to the blackboard.
+
+ Args:
+ child: the child behaviour or subtree
+ story_element_type: the element type [act,scene,maneuver,event,action]
+ element_name: the story element's name attribute
+ """
+
+ def __init__(self, child, story_element_type, element_name):
+ super(StoryElementStatusToBlackboard, self).__init__(name=child.name, child=child)
+ self.story_element_type = story_element_type
+ self.element_name = element_name
+ self.blackboard = py_trees.blackboard.Blackboard()
+
+ def initialise(self):
+ """
+ Record the elements's start time on the blackboard
+ """
+ self.blackboard.set(
+ name="({}){}-{}".format(self.story_element_type.upper(),
+ self.element_name, "START"),
+ value=GameTime.get_time(),
+ overwrite=True
+ )
+
+ def update(self):
+ """
+ Reflect the decorated child's status
+ Returns: the decorated child's status
+ """
+ return self.decorated.status
+
+ def terminate(self, new_status):
+ """
+ Terminate and mark Blackboard entry with END
+ """
+ # Report whether we ended with End or Cancel
+ # If we were ended or cancelled, our state will be INVALID and
+ # We will have an ancestor (a parallel SUCCESS_ON_ALL) with a successful child/children
+ # It's possible we ENDed AND CANCELled if both condition groups were true simultaneously
+ # NOTE 'py_trees.common.Status.INVALID' is the status of a behaviur which was terminated by a parent
+ rules = []
+ if new_status == py_trees.common.Status.INVALID:
+ # We were terminated from above unnaturally
+ # Figure out if were ended or cancelled
+ terminating_ancestor = self.parent
+ while terminating_ancestor.status == py_trees.common.Status.INVALID:
+ terminating_ancestor = terminating_ancestor.parent
+ # We have found an ancestory which was not terminated by a parent
+ # Check what caused it to terminate its children
+ if terminating_ancestor.status == py_trees.common.Status.SUCCESS:
+ successful_children = [
+ child.name
+ for child
+ in terminating_ancestor.children
+ if child.status == py_trees.common.Status.SUCCESS]
+ if "StopTrigger" in successful_children:
+ rules.append("END")
+
+ # END is the default status unless we have a more detailed one
+ rules = rules or ["END"]
+
+ for rule in rules:
+ self.blackboard.set(
+ name="({}){}-{}".format(self.story_element_type.upper(),
+ self.element_name, rule),
+ value=GameTime.get_time(),
+ overwrite=True
+ )
+
+
+def get_xml_path(tree, node):
+ """
+ Extract the full path of a node within an XML tree
+
+ Note: Catalogs are pulled from a separate file so the XML tree is split.
+ This means that in order to get the XML path, it must be done in 2 steps.
+ Some places in this python script do that by concatenating the results
+ of 2 get_xml_path calls with another ">".
+ Example: "Behavior>AutopilotSequence" + ">" + "StartAutopilot>StartAutopilot>StartAutopilot"
+ """
+
+ path = ""
+ parent_map = {c: p for p in tree.iter() for c in p}
+
+ cur_node = node
+ while cur_node != tree:
+ path = "{}>{}".format(cur_node.attrib.get('name'), path)
+ cur_node = parent_map[cur_node]
+
+ path = path[:-1]
+ return path
+
+
+class OpenScenario(BasicScenario):
+
+ """
+ Implementation of the OpenSCENARIO scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, config_file, debug_mode=False, criteria_enable=True, timeout=300):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self.config = config
+ self.route = None
+ self.config_file = config_file
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(OpenScenario, self).__init__("OpenScenario", ego_vehicles=ego_vehicles, config=config,
+ world=world, debug_mode=debug_mode,
+ terminate_on_failure=False, criteria_enable=criteria_enable)
+
+ def _initialize_environment(self, world):
+ """
+ Initialization of weather and road friction.
+ """
+ pass
+
+ def _create_environment_behavior(self):
+ # Set the appropriate weather conditions
+
+ env_behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="EnvironmentBehavior")
+
+ weather_update = ChangeWeather(
+ OpenScenarioParser.get_weather_from_env_action(self.config.init, self.config.catalogs))
+ road_friction = ChangeRoadFriction(
+ OpenScenarioParser.get_friction_from_env_action(self.config.init, self.config.catalogs))
+ env_behavior.add_child(oneshot_behavior(variable_name="InitialWeather", behaviour=weather_update))
+ env_behavior.add_child(oneshot_behavior(variable_name="InitRoadFriction", behaviour=road_friction))
+
+ return env_behavior
+
+ def _create_init_behavior(self):
+
+ init_behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="InitBehaviour")
+
+ for actor in self.config.other_actors + self.config.ego_vehicles:
+ for carla_actor in self.other_actors + self.ego_vehicles:
+ if 'role_name' in carla_actor.attributes and carla_actor.attributes['role_name'] == actor.rolename:
+ actor_init_behavior = py_trees.composites.Sequence(name="InitActor{}".format(actor.rolename))
+
+ controller_atomic = None
+
+ for private in self.config.init.iter("Private"):
+ if private.attrib.get('entityRef', None) == actor.rolename:
+ for private_action in private.iter("PrivateAction"):
+ for controller_action in private_action.iter('ControllerAction'):
+ module, args = OpenScenarioParser.get_controller(
+ controller_action, self.config.catalogs)
+ controller_atomic = ChangeActorControl(
+ carla_actor, control_py_module=module, args=args)
+
+ if controller_atomic is None:
+ controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={})
+
+ actor_init_behavior.add_child(controller_atomic)
+
+ if actor.speed > 0:
+ actor_init_behavior.add_child(ChangeActorTargetSpeed(carla_actor, actor.speed, init_speed=True))
+
+ init_behavior.add_child(actor_init_behavior)
+ break
+
+ return init_behavior
+
+ def _create_behavior(self):
+ """
+ Basic behavior do nothing, i.e. Idle
+ """
+
+ story_behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Story")
+
+ joint_actor_list = self.other_actors + self.ego_vehicles + [None]
+
+ for act in self.config.story.iter("Act"):
+
+ act_sequence = py_trees.composites.Sequence(
+ name="Act StartConditions and behaviours")
+
+ start_conditions = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="StartConditions Group")
+
+ parallel_behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="Maneuver + EndConditions Group")
+
+ parallel_sequences = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Maneuvers")
+
+ for sequence in act.iter("ManeuverGroup"):
+ sequence_behavior = py_trees.composites.Sequence(name=sequence.attrib.get('name'))
+ repetitions = sequence.attrib.get('maximumExecutionCount', 1)
+
+ for _ in range(int(repetitions)):
+
+ actor_ids = []
+ for actor in sequence.iter("Actors"):
+ for entity in actor.iter("EntityRef"):
+ entity_name = entity.attrib.get('entityRef', None)
+ for k, _ in enumerate(joint_actor_list):
+ if joint_actor_list[k] and entity_name == joint_actor_list[k].attributes['role_name']:
+ actor_ids.append(k)
+ break
+
+ if not actor_ids:
+ print("Warning: Maneuvergroup {} does not use reference actors!".format(
+ sequence.attrib.get('name')))
+ actor_ids.append(len(joint_actor_list) - 1)
+
+ # Collect catalog reference maneuvers in order to process them at the same time as normal maneuvers
+ catalog_maneuver_list = []
+ for catalog_reference in sequence.iter("CatalogReference"):
+ catalog_maneuver = OpenScenarioParser.get_catalog_entry(self.config.catalogs, catalog_reference)
+ catalog_maneuver_list.append(catalog_maneuver)
+ all_maneuvers = itertools.chain(iter(catalog_maneuver_list), sequence.iter("Maneuver"))
+ single_sequence_iteration = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=sequence_behavior.name)
+ for maneuver in all_maneuvers: # Iterates through both CatalogReferences and Maneuvers
+ maneuver_parallel = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL,
+ name="Maneuver " + maneuver.attrib.get('name'))
+ for event in maneuver.iter("Event"):
+ event_sequence = py_trees.composites.Sequence(
+ name="Event " + event.attrib.get('name'))
+ parallel_actions = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="Actions")
+ for child in event.iter():
+ if child.tag == "Action":
+ for actor_id in actor_ids:
+ maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic(
+ child, joint_actor_list[actor_id], self.config.catalogs)
+ maneuver_behavior = StoryElementStatusToBlackboard(
+ maneuver_behavior, "ACTION", child.attrib.get('name'))
+ parallel_actions.add_child(
+ oneshot_behavior(variable_name=# See note in get_xml_path
+ get_xml_path(self.config.story, sequence) + '>' + \
+ get_xml_path(maneuver, child),
+ behaviour=maneuver_behavior))
+
+ if child.tag == "StartTrigger":
+ # There is always one StartConditions block per Event
+ parallel_condition_groups = self._create_condition_container(
+ child, "Parallel Condition Groups", sequence, maneuver)
+ event_sequence.add_child(
+ parallel_condition_groups)
+
+ parallel_actions = StoryElementStatusToBlackboard(
+ parallel_actions, "EVENT", event.attrib.get('name'))
+ event_sequence.add_child(parallel_actions)
+ maneuver_parallel.add_child(
+ oneshot_behavior(variable_name=get_xml_path(self.config.story, sequence) + '>' +
+ get_xml_path(maneuver, event), # See get_xml_path
+ behaviour=event_sequence))
+ maneuver_parallel = StoryElementStatusToBlackboard(
+ maneuver_parallel, "MANEUVER", maneuver.attrib.get('name'))
+ single_sequence_iteration.add_child(
+ oneshot_behavior(variable_name=get_xml_path(self.config.story, sequence) + '>' +
+ maneuver.attrib.get('name'), # See get_xml_path
+ behaviour=maneuver_parallel))
+
+ # OpenSCENARIO refers to Sequences as Scenes in this instance
+ single_sequence_iteration = StoryElementStatusToBlackboard(
+ single_sequence_iteration, "SCENE", sequence.attrib.get('name'))
+ single_sequence_iteration = repeatable_behavior(
+ single_sequence_iteration, get_xml_path(self.config.story, sequence))
+
+ sequence_behavior.add_child(single_sequence_iteration)
+
+ if sequence_behavior.children:
+ parallel_sequences.add_child(
+ oneshot_behavior(variable_name=get_xml_path(self.config.story, sequence),
+ behaviour=sequence_behavior))
+
+ if parallel_sequences.children:
+ parallel_sequences = StoryElementStatusToBlackboard(
+ parallel_sequences, "ACT", act.attrib.get('name'))
+ parallel_behavior.add_child(parallel_sequences)
+
+ start_triggers = act.find("StartTrigger")
+ if list(start_triggers) is not None:
+ for start_condition in start_triggers:
+ parallel_start_criteria = self._create_condition_container(start_condition, "StartConditions")
+ if parallel_start_criteria.children:
+ start_conditions.add_child(parallel_start_criteria)
+ end_triggers = act.find("StopTrigger")
+ if end_triggers is not None and list(end_triggers) is not None:
+ for end_condition in end_triggers:
+ parallel_end_criteria = self._create_condition_container(
+ end_condition, "EndConditions", success_on_all=False)
+ if parallel_end_criteria.children:
+ parallel_behavior.add_child(parallel_end_criteria)
+
+ if start_conditions.children:
+ act_sequence.add_child(start_conditions)
+ if parallel_behavior.children:
+ act_sequence.add_child(parallel_behavior)
+
+ if act_sequence.children:
+ story_behavior.add_child(act_sequence)
+
+ # Build behavior tree
+ behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="behavior")
+
+ env_behavior = self._create_environment_behavior()
+ if env_behavior is not None:
+ behavior.add_child(oneshot_behavior(variable_name="InitialEnvironmentSettings", behaviour=env_behavior))
+
+ init_behavior = self._create_init_behavior()
+ if init_behavior is not None:
+ behavior.add_child(oneshot_behavior(variable_name="InitialActorSettings", behaviour=init_behavior))
+
+ behavior.add_child(story_behavior)
+
+ return behavior
+
+ def _create_condition_container(self, node, name='Conditions Group', sequence=None,
+ maneuver=None, success_on_all=True):
+ """
+ This is a generic function to handle conditions utilising ConditionGroups
+ Each ConditionGroup is represented as a Sequence of Conditions
+ The ConditionGroups are grouped under a SUCCESS_ON_ONE Parallel
+ """
+
+ parallel_condition_groups = py_trees.composites.Parallel(name,
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ for condition_group in node.iter("ConditionGroup"):
+ if success_on_all:
+ condition_group_sequence = py_trees.composites.Parallel(
+ name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+ else:
+ condition_group_sequence = py_trees.composites.Parallel(
+ name="Condition Group", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ for condition in condition_group.iter("Condition"):
+ criterion = OpenScenarioParser.convert_condition_to_atomic(
+ condition, self.other_actors + self.ego_vehicles)
+ if sequence is not None and maneuver is not None:
+ xml_path = get_xml_path(self.config.story, sequence) + '>' + \
+ get_xml_path(maneuver, condition) # See note in get_xml_path
+ else:
+ xml_path = get_xml_path(self.config.story, condition)
+ criterion = oneshot_behavior(variable_name=xml_path, behaviour=criterion)
+ condition_group_sequence.add_child(criterion)
+
+ if condition_group_sequence.children:
+ parallel_condition_groups.add_child(condition_group_sequence)
+
+ return parallel_condition_groups
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ parallel_criteria = py_trees.composites.Parallel("EndConditions (Criteria Group)",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ criteria = []
+ for endcondition in self.config.storyboard.iter("StopTrigger"):
+ for condition in endcondition.iter("Condition"):
+ if condition.attrib.get('name').startswith('criteria_'):
+ condition.set('name', condition.attrib.get('name')[9:])
+ criteria.append(condition)
+
+ for condition in criteria:
+ criterion = OpenScenarioParser.convert_condition_to_atomic(condition, self.ego_vehicles)
+ parallel_criteria.add_child(criterion)
+
+ return parallel_criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/opposite_vehicle_taking_priority.py b/scenario_runner/srunner/scenarios/opposite_vehicle_taking_priority.py
new file mode 100644
index 0000000..916fadc
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/opposite_vehicle_taking_priority.py
@@ -0,0 +1,227 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2018-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Scenarios in which another (opposite) vehicle 'illegally' takes
+priority, e.g. by running a red traffic light.
+"""
+
+from __future__ import print_function
+import sys
+
+import py_trees
+import carla
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ WaypointFollower,
+ SyncArrival)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest, DrivenDistanceTest, MaxVelocityTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToLocation,
+ InTriggerDistanceToNextIntersection,
+ DriveDistance)
+from srunner.scenariomanager.timer import TimeOut
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import (get_crossing_point,
+ get_geometric_linear_intersection,
+ generate_target_waypoint_list)
+
+
+class OppositeVehicleRunningRedLight(BasicScenario):
+
+ """
+ This class holds everything required for a scenario,
+ in which an other vehicle takes priority from the ego
+ vehicle, by running a red traffic light (while the ego
+ vehicle has green) (Traffic Scenario 7)
+
+ This is a single ego vehicle scenario
+ """
+
+ # ego vehicle parameters
+ _ego_max_velocity_allowed = 20 # Maximum allowed velocity [m/s]
+ _ego_avg_velocity_expected = 4 # Average expected velocity [m/s]
+ _ego_expected_driven_distance = 70 # Expected driven distance [m]
+ _ego_distance_to_traffic_light = 32 # Trigger distance to traffic light [m]
+ _ego_distance_to_drive = 40 # Allowed distance to drive
+
+ # other vehicle
+ _other_actor_target_velocity = 10 # Target velocity of other vehicle
+ _other_actor_max_brake = 1.0 # Maximum brake of other vehicle
+ _other_actor_distance = 50 # Distance the other vehicle should drive
+
+ _traffic_light = None
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=180):
+ """
+ Setup all relevant parameters and create scenario
+ and instantiate scenario manager
+ """
+
+ self._other_actor_transform = None
+
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(OppositeVehicleRunningRedLight, self).__init__("OppositeVehicleRunningRedLight",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ self._traffic_light = CarlaDataProvider.get_next_traffic_light(self.ego_vehicles[0], False)
+
+ if self._traffic_light is None:
+ print("No traffic light for the given location of the ego vehicle found")
+ sys.exit(-1)
+
+ self._traffic_light.set_state(carla.TrafficLightState.Green)
+ self._traffic_light.set_green_time(self.timeout)
+
+ # other vehicle's traffic light
+ traffic_light_other = CarlaDataProvider.get_next_traffic_light(self.other_actors[0], False)
+
+ if traffic_light_other is None:
+ print("No traffic light for the given location of the other vehicle found")
+ sys.exit(-1)
+
+ traffic_light_other.set_state(carla.TrafficLightState.Red)
+ traffic_light_other.set_red_time(self.timeout)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ self._other_actor_transform = config.other_actors[0].transform
+ first_vehicle_transform = carla.Transform(
+ carla.Location(config.other_actors[0].transform.location.x,
+ config.other_actors[0].transform.location.y,
+ config.other_actors[0].transform.location.z),
+ config.other_actors[0].transform.rotation)
+ first_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, first_vehicle_transform)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ Scenario behavior:
+ The other vehicle waits until the ego vehicle is close enough to the
+ intersection and that its own traffic light is red. Then, it will start
+ driving and 'illegally' cross the intersection. After a short distance
+ it should stop again, outside of the intersection. The ego vehicle has
+ to avoid the crash, but continue driving after the intersection is clear.
+
+ If this does not happen within 120 seconds, a timeout stops the scenario
+ """
+ crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0])
+
+ # start condition
+ startcondition = InTriggerDistanceToLocation(
+ self.ego_vehicles[0],
+ crossing_point_dynamic,
+ self._ego_distance_to_traffic_light,
+ name="Waiting for start position")
+
+ sync_arrival_parallel = py_trees.composites.Parallel(
+ "Synchronize arrival times",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ location_of_collision_dynamic = get_geometric_linear_intersection(self.ego_vehicles[0], self.other_actors[0])
+
+ sync_arrival = SyncArrival(
+ self.other_actors[0], self.ego_vehicles[0], location_of_collision_dynamic)
+ sync_arrival_stop = InTriggerDistanceToNextIntersection(self.other_actors[0],
+ 5)
+ sync_arrival_parallel.add_child(sync_arrival)
+ sync_arrival_parallel.add_child(sync_arrival_stop)
+
+ # Generate plan for WaypointFollower
+ turn = 0 # drive straight ahead
+ plan = []
+
+ # generating waypoints until intersection (target_waypoint)
+ plan, target_waypoint = generate_target_waypoint_list(
+ CarlaDataProvider.get_map().get_waypoint(self.other_actors[0].get_location()), turn)
+
+ # Generating waypoint list till next intersection
+ wp_choice = target_waypoint.next(5.0)
+ while len(wp_choice) == 1:
+ target_waypoint = wp_choice[0]
+ plan.append((target_waypoint, RoadOption.LANEFOLLOW))
+ wp_choice = target_waypoint.next(5.0)
+
+ continue_driving = py_trees.composites.Parallel(
+ "ContinueDriving",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ continue_driving_waypoints = WaypointFollower(
+ self.other_actors[0], self._other_actor_target_velocity, plan=plan, avoid_collision=False)
+
+ continue_driving_distance = DriveDistance(
+ self.other_actors[0],
+ self._other_actor_distance,
+ name="Distance")
+
+ continue_driving_timeout = TimeOut(10)
+
+ continue_driving.add_child(continue_driving_waypoints)
+ continue_driving.add_child(continue_driving_distance)
+ continue_driving.add_child(continue_driving_timeout)
+
+ # finally wait that ego vehicle drove a specific distance
+ wait = DriveDistance(
+ self.ego_vehicles[0],
+ self._ego_distance_to_drive,
+ name="DriveDistance")
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("Sequence Behavior")
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform))
+ sequence.add_child(startcondition)
+ sequence.add_child(sync_arrival_parallel)
+ sequence.add_child(continue_driving)
+ sequence.add_child(wait)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ max_velocity_criterion = MaxVelocityTest(
+ self.ego_vehicles[0],
+ self._ego_max_velocity_allowed,
+ optional=True)
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ driven_distance_criterion = DrivenDistanceTest(
+ self.ego_vehicles[0],
+ self._ego_expected_driven_distance)
+
+ criteria.append(max_velocity_criterion)
+ criteria.append(collision_criterion)
+ criteria.append(driven_distance_criterion)
+
+ # Add the collision and lane checks for all vehicles as well
+ for vehicle in self.other_actors:
+ collision_criterion = CollisionTest(vehicle)
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors and traffic lights upon deletion
+ """
+ self._traffic_light = None
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/other_leading_vehicle.py b/scenario_runner/srunner/scenarios/other_leading_vehicle.py
new file mode 100644
index 0000000..fb6da7e
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/other_leading_vehicle.py
@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Other Leading Vehicle scenario:
+
+The scenario realizes a common driving behavior, in which the
+user-controlled ego vehicle follows a leading car driving down
+a given road. At some point the leading car has to decelerate.
+The ego vehicle has to react accordingly by changing lane to avoid a
+collision and follow the leading car in other lane. The scenario ends
+either via a timeout, or if the ego vehicle drives some distance.
+"""
+
+import py_trees
+
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ WaypointFollower,
+ ActorDestroy)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToVehicle,
+ DriveDistance)
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import get_waypoint_in_distance
+
+
+class OtherLeadingVehicle(BasicScenario):
+
+ """
+ This class holds everything required for a simple "Other Leading Vehicle"
+ scenario involving a user controlled vehicle and two other actors.
+ Traffic Scenario 05
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=80):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._world = world
+ self._map = CarlaDataProvider.get_map()
+ self._first_vehicle_location = 35
+ self._second_vehicle_location = self._first_vehicle_location + 1
+ self._ego_vehicle_drive_distance = self._first_vehicle_location * 4
+ self._first_vehicle_speed = 55
+ self._second_vehicle_speed = 45
+ self._reference_waypoint = self._map.get_waypoint(config.trigger_points[0].location)
+ self._other_actor_max_brake = 1.0
+ self._first_actor_transform = None
+ self._second_actor_transform = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+
+ super(OtherLeadingVehicle, self).__init__("VehicleDeceleratingInMultiLaneSetUp",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ first_vehicle_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._first_vehicle_location)
+ second_vehicle_waypoint, _ = get_waypoint_in_distance(self._reference_waypoint, self._second_vehicle_location)
+ second_vehicle_waypoint = second_vehicle_waypoint.get_left_lane()
+
+ first_vehicle_transform = carla.Transform(first_vehicle_waypoint.transform.location,
+ first_vehicle_waypoint.transform.rotation)
+ second_vehicle_transform = carla.Transform(second_vehicle_waypoint.transform.location,
+ second_vehicle_waypoint.transform.rotation)
+
+ first_vehicle = CarlaDataProvider.request_new_actor('vehicle.nissan.patrol', first_vehicle_transform)
+ second_vehicle = CarlaDataProvider.request_new_actor('vehicle.audi.tt', second_vehicle_transform)
+
+ self.other_actors.append(first_vehicle)
+ self.other_actors.append(second_vehicle)
+
+ self._first_actor_transform = first_vehicle_transform
+ self._second_actor_transform = second_vehicle_transform
+
+ def _create_behavior(self):
+ """
+ The scenario defined after is a "other leading vehicle" scenario. After
+ invoking this scenario, the user controlled vehicle has to drive towards the
+ moving other actors, then make the leading actor to decelerate when user controlled
+ vehicle is at some close distance. Finally, the user-controlled vehicle has to change
+ lane to avoid collision and follow other leading actor in other lane to end the scenario.
+ If this does not happen within 90 seconds, a timeout stops the scenario or the ego vehicle
+ drives certain distance and stops the scenario.
+ """
+ # start condition
+ driving_in_same_direction = py_trees.composites.Parallel("All actors driving in same direction",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ leading_actor_sequence_behavior = py_trees.composites.Sequence("Decelerating actor sequence behavior")
+
+ # both actors moving in same direction
+ keep_velocity = py_trees.composites.Parallel("Trigger condition for deceleration",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ keep_velocity.add_child(WaypointFollower(self.other_actors[0], self._first_vehicle_speed, avoid_collision=True))
+ keep_velocity.add_child(InTriggerDistanceToVehicle(self.other_actors[0], self.ego_vehicles[0], 55))
+
+ # Decelerating actor sequence behavior
+ decelerate = self._first_vehicle_speed / 3.2
+ leading_actor_sequence_behavior.add_child(keep_velocity)
+ leading_actor_sequence_behavior.add_child(WaypointFollower(self.other_actors[0], decelerate,
+ avoid_collision=True))
+ # end condition
+ ego_drive_distance = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_drive_distance)
+
+ # Build behavior tree
+ sequence = py_trees.composites.Sequence("Scenario behavior")
+ parallel_root = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ parallel_root.add_child(ego_drive_distance)
+ parallel_root.add_child(driving_in_same_direction)
+ driving_in_same_direction.add_child(leading_actor_sequence_behavior)
+ driving_in_same_direction.add_child(WaypointFollower(self.other_actors[1], self._second_vehicle_speed,
+ avoid_collision=True))
+
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self._first_actor_transform))
+ sequence.add_child(ActorTransformSetter(self.other_actors[1], self._second_actor_transform))
+ sequence.add_child(parallel_root)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+ sequence.add_child(ActorDestroy(self.other_actors[1]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collision_criterion)
+
+ return criteria
+
+ def __del__(self):
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/route_scenario.py b/scenario_runner/srunner/scenarios/route_scenario.py
new file mode 100644
index 0000000..5db8414
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/route_scenario.py
@@ -0,0 +1,515 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides Challenge routes as standalone scenarios
+"""
+
+from __future__ import print_function
+
+import math
+import traceback
+import xml.etree.ElementTree as ET
+import numpy.random as random
+
+import py_trees
+
+import carla
+
+from agents.navigation.local_planner import RoadOption
+
+# pylint: disable=line-too-long
+from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData
+# pylint: enable=line-too-long
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import Idle, ScenarioTriggerer
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.route_parser import RouteParser, TRIGGER_THRESHOLD, TRIGGER_ANGLE_THRESHOLD
+from srunner.tools.route_manipulation import interpolate_trajectory
+from srunner.tools.py_trees_port import oneshot_behavior
+
+from srunner.scenarios.control_loss import ControlLoss
+from srunner.scenarios.follow_leading_vehicle import FollowLeadingVehicle
+from srunner.scenarios.object_crash_vehicle import DynamicObjectCrossing
+from srunner.scenarios.object_crash_intersection import VehicleTurningRoute
+from srunner.scenarios.other_leading_vehicle import OtherLeadingVehicle
+from srunner.scenarios.maneuver_opposite_direction import ManeuverOppositeDirection
+from srunner.scenarios.junction_crossing_route import SignalJunctionCrossingRoute, NoSignalJunctionCrossingRoute
+
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest,
+ InRouteTest,
+ RouteCompletionTest,
+ OutsideRouteLanesTest,
+ RunningRedLightTest,
+ RunningStopTest,
+ ActorSpeedAboveThresholdTest)
+
+SECONDS_GIVEN_PER_METERS = 0.4
+
+NUMBER_CLASS_TRANSLATION = {
+ "Scenario1": ControlLoss,
+ "Scenario2": FollowLeadingVehicle,
+ "Scenario3": DynamicObjectCrossing,
+ "Scenario4": VehicleTurningRoute,
+ "Scenario5": OtherLeadingVehicle,
+ "Scenario6": ManeuverOppositeDirection,
+ "Scenario7": SignalJunctionCrossingRoute,
+ "Scenario8": SignalJunctionCrossingRoute,
+ "Scenario9": SignalJunctionCrossingRoute,
+ "Scenario10": NoSignalJunctionCrossingRoute
+}
+
+
+def convert_json_to_transform(actor_dict):
+ """
+ Convert a JSON string to a CARLA transform
+ """
+ return carla.Transform(location=carla.Location(x=float(actor_dict['x']), y=float(actor_dict['y']),
+ z=float(actor_dict['z'])),
+ rotation=carla.Rotation(roll=0.0, pitch=0.0, yaw=float(actor_dict['yaw'])))
+
+
+def convert_json_to_actor(actor_dict):
+ """
+ Convert a JSON string to an ActorConfigurationData dictionary
+ """
+ node = ET.Element('waypoint')
+ node.set('x', actor_dict['x'])
+ node.set('y', actor_dict['y'])
+ node.set('z', actor_dict['z'])
+ node.set('yaw', actor_dict['yaw'])
+
+ return ActorConfigurationData.parse_from_node(node, 'simulation')
+
+
+def convert_transform_to_location(transform_vec):
+ """
+ Convert a vector of transforms to a vector of locations
+ """
+ location_vec = []
+ for transform_tuple in transform_vec:
+ location_vec.append((transform_tuple[0].location, transform_tuple[1]))
+
+ return location_vec
+
+
+def compare_scenarios(scenario_choice, existent_scenario):
+ """
+ Compare function for scenarios based on distance of the scenario start position
+ """
+ def transform_to_pos_vec(scenario):
+ """
+ Convert left/right/front to a meaningful CARLA position
+ """
+ position_vec = [scenario['trigger_position']]
+ if scenario['other_actors'] is not None:
+ if 'left' in scenario['other_actors']:
+ position_vec += scenario['other_actors']['left']
+ if 'front' in scenario['other_actors']:
+ position_vec += scenario['other_actors']['front']
+ if 'right' in scenario['other_actors']:
+ position_vec += scenario['other_actors']['right']
+
+ return position_vec
+
+ # put the positions of the scenario choice into a vec of positions to be able to compare
+
+ choice_vec = transform_to_pos_vec(scenario_choice)
+ existent_vec = transform_to_pos_vec(existent_scenario)
+ for pos_choice in choice_vec:
+ for pos_existent in existent_vec:
+
+ dx = float(pos_choice['x']) - float(pos_existent['x'])
+ dy = float(pos_choice['y']) - float(pos_existent['y'])
+ dz = float(pos_choice['z']) - float(pos_existent['z'])
+ dist_position = math.sqrt(dx * dx + dy * dy + dz * dz)
+ dyaw = float(pos_choice['yaw']) - float(pos_choice['yaw'])
+ dist_angle = math.sqrt(dyaw * dyaw)
+ if dist_position < TRIGGER_THRESHOLD and dist_angle < TRIGGER_ANGLE_THRESHOLD:
+ return True
+
+ return False
+
+
+class RouteScenario(BasicScenario):
+
+ """
+ Implementation of a RouteScenario, i.e. a scenario that consists of driving along a pre-defined route,
+ along which several smaller scenarios are triggered
+ """
+
+ def __init__(self, world, config, debug_mode=False, criteria_enable=True, timeout=300):
+ """
+ Setup all relevant parameters and create scenarios along route
+ """
+
+ self.config = config
+ self.route = None
+ self.sampled_scenarios_definitions = None
+
+ self._update_route(world, config, debug_mode)
+
+ ego_vehicle = self._update_ego_vehicle()
+
+ self.list_scenarios = self._build_scenario_instances(world,
+ ego_vehicle,
+ self.sampled_scenarios_definitions,
+ scenarios_per_tick=5,
+ timeout=self.timeout,
+ debug_mode=debug_mode)
+
+ super(RouteScenario, self).__init__(name=config.name,
+ ego_vehicles=[ego_vehicle],
+ config=config,
+ world=world,
+ debug_mode=False,
+ terminate_on_failure=False,
+ criteria_enable=criteria_enable)
+
+ def _update_route(self, world, config, debug_mode):
+ """
+ Update the input route, i.e. refine waypoint list, and extract possible scenario locations
+
+ Parameters:
+ - world: CARLA world
+ - config: Scenario configuration (RouteConfiguration)
+ """
+
+ # Transform the scenario file into a dictionary
+ world_annotations = RouteParser.parse_annotations_file(config.scenario_file)
+
+ # prepare route's trajectory (interpolate and add the GPS route)
+ gps_route, route = interpolate_trajectory(world, config.trajectory)
+
+ potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(config.town, route, world_annotations)
+
+ self.route = route
+ CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route))
+
+ config.agent.set_global_plan(gps_route, self.route)
+
+ # Sample the scenarios to be used for this route instance.
+ self.sampled_scenarios_definitions = self._scenario_sampling(potential_scenarios_definitions)
+
+ # Timeout of scenario in seconds
+ self.timeout = self._estimate_route_timeout()
+
+ # Print route in debug mode
+ if debug_mode:
+ self._draw_waypoints(world, self.route, vertical_shift=1.0, persistency=50000.0)
+
+ def _update_ego_vehicle(self):
+ """
+ Set/Update the start position of the ego_vehicle
+ """
+ # move ego to correct position
+ elevate_transform = self.route[0][0]
+ elevate_transform.location.z += 0.5
+
+ ego_vehicle = CarlaDataProvider.request_new_actor('vehicle.lincoln.mkz2017',
+ elevate_transform,
+ rolename='hero')
+
+ return ego_vehicle
+
+ def _estimate_route_timeout(self):
+ """
+ Estimate the duration of the route
+ """
+ route_length = 0.0 # in meters
+
+ prev_point = self.route[0][0]
+ for current_point, _ in self.route[1:]:
+ dist = current_point.location.distance(prev_point.location)
+ route_length += dist
+ prev_point = current_point
+
+ return int(SECONDS_GIVEN_PER_METERS * route_length)
+
+ # pylint: disable=no-self-use
+ def _draw_waypoints(self, world, waypoints, vertical_shift, persistency=-1):
+ """
+ Draw a list of waypoints at a certain height given in vertical_shift.
+ """
+ for w in waypoints:
+ wp = w[0].location + carla.Location(z=vertical_shift)
+
+ size = 0.2
+ if w[1] == RoadOption.LEFT: # Yellow
+ color = carla.Color(255, 255, 0)
+ elif w[1] == RoadOption.RIGHT: # Cyan
+ color = carla.Color(0, 255, 255)
+ elif w[1] == RoadOption.CHANGELANELEFT: # Orange
+ color = carla.Color(255, 64, 0)
+ elif w[1] == RoadOption.CHANGELANERIGHT: # Dark Cyan
+ color = carla.Color(0, 64, 255)
+ elif w[1] == RoadOption.STRAIGHT: # Gray
+ color = carla.Color(128, 128, 128)
+ else: # LANEFOLLOW
+ color = carla.Color(0, 255, 0) # Green
+ size = 0.1
+
+ world.debug.draw_point(wp, size=size, color=color, life_time=persistency)
+
+ world.debug.draw_point(waypoints[0][0].location + carla.Location(z=vertical_shift), size=0.2,
+ color=carla.Color(0, 0, 255), life_time=persistency)
+ world.debug.draw_point(waypoints[-1][0].location + carla.Location(z=vertical_shift), size=0.2,
+ color=carla.Color(255, 0, 0), life_time=persistency)
+
+ def _scenario_sampling(self, potential_scenarios_definitions, random_seed=0):
+ """
+ The function used to sample the scenarios that are going to happen for this route.
+ """
+
+ # fix the random seed for reproducibility
+ rng = random.RandomState(random_seed)
+
+ def position_sampled(scenario_choice, sampled_scenarios):
+ """
+ Check if a position was already sampled, i.e. used for another scenario
+ """
+ for existent_scenario in sampled_scenarios:
+ # If the scenarios have equal positions then it is true.
+ if compare_scenarios(scenario_choice, existent_scenario):
+ return True
+
+ return False
+
+ # The idea is to randomly sample a scenario per trigger position.
+ sampled_scenarios = []
+ for trigger in potential_scenarios_definitions.keys():
+ possible_scenarios = potential_scenarios_definitions[trigger]
+
+ scenario_choice = rng.choice(possible_scenarios)
+ del possible_scenarios[possible_scenarios.index(scenario_choice)]
+ # We keep sampling and testing if this position is present on any of the scenarios.
+ while position_sampled(scenario_choice, sampled_scenarios):
+ if possible_scenarios is None or not possible_scenarios:
+ scenario_choice = None
+ break
+ scenario_choice = rng.choice(possible_scenarios)
+ del possible_scenarios[possible_scenarios.index(scenario_choice)]
+
+ if scenario_choice is not None:
+ sampled_scenarios.append(scenario_choice)
+
+ return sampled_scenarios
+
+ def _build_scenario_instances(self, world, ego_vehicle, scenario_definitions,
+ scenarios_per_tick=5, timeout=300, debug_mode=False):
+ """
+ Based on the parsed route and possible scenarios, build all the scenario classes.
+ """
+ scenario_instance_vec = []
+
+ if debug_mode:
+ for scenario in scenario_definitions:
+ loc = carla.Location(scenario['trigger_position']['x'],
+ scenario['trigger_position']['y'],
+ scenario['trigger_position']['z']) + carla.Location(z=2.0)
+ world.debug.draw_point(loc, size=0.3, color=carla.Color(255, 0, 0), life_time=100000)
+ world.debug.draw_string(loc, str(scenario['name']), draw_shadow=False,
+ color=carla.Color(0, 0, 255), life_time=100000, persistent_lines=True)
+
+ for scenario_number, definition in enumerate(scenario_definitions):
+ # Get the class possibilities for this scenario number
+ scenario_class = NUMBER_CLASS_TRANSLATION[definition['name']]
+
+ # Create the other actors that are going to appear
+ if definition['other_actors'] is not None:
+ list_of_actor_conf_instances = self._get_actors_instances(definition['other_actors'])
+ else:
+ list_of_actor_conf_instances = []
+ # Create an actor configuration for the ego-vehicle trigger position
+
+ egoactor_trigger_position = convert_json_to_transform(definition['trigger_position'])
+ scenario_configuration = ScenarioConfiguration()
+ scenario_configuration.other_actors = list_of_actor_conf_instances
+ scenario_configuration.trigger_points = [egoactor_trigger_position]
+ scenario_configuration.subtype = definition['scenario_type']
+ scenario_configuration.ego_vehicles = [ActorConfigurationData('vehicle.lincoln.mkz2017',
+ ego_vehicle.get_transform(),
+ 'hero')]
+ route_var_name = "ScenarioRouteNumber{}".format(scenario_number)
+ scenario_configuration.route_var_name = route_var_name
+
+ try:
+ scenario_instance = scenario_class(world, [ego_vehicle], scenario_configuration,
+ criteria_enable=False, timeout=timeout)
+ # Do a tick every once in a while to avoid spawning everything at the same time
+ if scenario_number % scenarios_per_tick == 0:
+ if CarlaDataProvider.is_sync_mode():
+ world.tick()
+ else:
+ world.wait_for_tick()
+
+ scenario_number += 1
+ except Exception as e: # pylint: disable=broad-except
+ if debug_mode:
+ traceback.print_exc()
+ print("Skipping scenario '{}' due to setup error: {}".format(definition['name'], e))
+ continue
+
+ scenario_instance_vec.append(scenario_instance)
+
+ return scenario_instance_vec
+
+ def _get_actors_instances(self, list_of_antagonist_actors):
+ """
+ Get the full list of actor instances.
+ """
+
+ def get_actors_from_list(list_of_actor_def):
+ """
+ Receives a list of actor definitions and creates an actual list of ActorConfigurationObjects
+ """
+ sublist_of_actors = []
+ for actor_def in list_of_actor_def:
+ sublist_of_actors.append(convert_json_to_actor(actor_def))
+
+ return sublist_of_actors
+
+ list_of_actors = []
+ # Parse vehicles to the left
+ if 'front' in list_of_antagonist_actors:
+ list_of_actors += get_actors_from_list(list_of_antagonist_actors['front'])
+
+ if 'left' in list_of_antagonist_actors:
+ list_of_actors += get_actors_from_list(list_of_antagonist_actors['left'])
+
+ if 'right' in list_of_antagonist_actors:
+ list_of_actors += get_actors_from_list(list_of_antagonist_actors['right'])
+
+ return list_of_actors
+
+ # pylint: enable=no-self-use
+
+ def _initialize_actors(self, config):
+ """
+ Set other_actors to the superset of all scenario actors
+ """
+
+ # Create the background activity of the route
+ town_amount = {
+ 'Town01': 120,
+ 'Town02': 100,
+ 'Town03': 120,
+ 'Town04': 200,
+ 'Town05': 120,
+ 'Town06': 150,
+ 'Town07': 110,
+ 'Town08': 180,
+ 'Town09': 300,
+ 'Town10': 120,
+ }
+
+ amount = town_amount[config.town] if config.town in town_amount else 0
+
+ new_actors = CarlaDataProvider.request_new_batch_actors('vehicle.*',
+ amount,
+ carla.Transform(),
+ autopilot=True,
+ random_location=True,
+ rolename='background')
+
+ if new_actors is None:
+ raise Exception("Error: Unable to add the background activity, all spawn points were occupied")
+
+ for _actor in new_actors:
+ self.other_actors.append(_actor)
+
+ # Add all the actors of the specific scenarios to self.other_actors
+ for scenario in self.list_scenarios:
+ self.other_actors.extend(scenario.other_actors)
+
+ def _create_behavior(self):
+ """
+ Basic behavior do nothing, i.e. Idle
+ """
+ scenario_trigger_distance = 1.5 # Max trigger distance between route and scenario
+
+ behavior = py_trees.composites.Parallel(policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+
+ subbehavior = py_trees.composites.Parallel(name="Behavior",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
+
+ scenario_behaviors = []
+ blackboard_list = []
+
+ for i, scenario in enumerate(self.list_scenarios):
+ if scenario.scenario.behavior is not None:
+ route_var_name = scenario.config.route_var_name
+ if route_var_name is not None:
+ scenario_behaviors.append(scenario.scenario.behavior)
+ blackboard_list.append([scenario.config.route_var_name,
+ scenario.config.trigger_points[0].location])
+ else:
+ name = "{} - {}".format(i, scenario.scenario.behavior.name)
+ oneshot_idiom = oneshot_behavior(name,
+ behaviour=scenario.scenario.behavior,
+ name=name)
+ scenario_behaviors.append(oneshot_idiom)
+
+ # Add behavior that manages the scenarios trigger conditions
+ scenario_triggerer = ScenarioTriggerer(
+ self.ego_vehicles[0],
+ self.route,
+ blackboard_list,
+ scenario_trigger_distance,
+ repeat_scenarios=False
+ )
+
+ subbehavior.add_child(scenario_triggerer) # make ScenarioTriggerer the first thing to be checked
+ subbehavior.add_children(scenario_behaviors)
+ subbehavior.add_child(Idle()) # The behaviours cannot make the route scenario stop
+ behavior.add_child(subbehavior)
+
+ return behavior
+
+ def _create_test_criteria(self):
+ """
+ """
+
+ criteria = []
+
+ route = convert_transform_to_location(self.route)
+
+ collision_criterion = CollisionTest(self.ego_vehicles[0], terminate_on_failure=False)
+
+ route_criterion = InRouteTest(self.ego_vehicles[0],
+ route=route,
+ offroad_max=30,
+ terminate_on_failure=True)
+
+ completion_criterion = RouteCompletionTest(self.ego_vehicles[0], route=route)
+
+ outsidelane_criterion = OutsideRouteLanesTest(self.ego_vehicles[0], route=route)
+
+ red_light_criterion = RunningRedLightTest(self.ego_vehicles[0])
+
+ stop_criterion = RunningStopTest(self.ego_vehicles[0])
+
+ blocked_criterion = ActorSpeedAboveThresholdTest(self.ego_vehicles[0],
+ speed_threshold=0.1,
+ below_threshold_max_time=90.0,
+ terminate_on_failure=True)
+
+ criteria.append(completion_criterion)
+ criteria.append(collision_criterion)
+ criteria.append(route_criterion)
+ criteria.append(outsidelane_criterion)
+ criteria.append(red_light_criterion)
+ criteria.append(stop_criterion)
+ criteria.append(blocked_criterion)
+
+ return criteria
+
+ def __del__(self):
+ """
+ Remove all actors upon deletion
+ """
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/signalized_junction_left_turn.py b/scenario_runner/srunner/scenarios/signalized_junction_left_turn.py
new file mode 100644
index 0000000..8e5bb51
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/signalized_junction_left_turn.py
@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Collection of traffic scenarios where the ego vehicle (hero)
+is making a left turn
+"""
+
+from six.moves.queue import Queue # pylint: disable=relative-import
+
+import py_trees
+import carla
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ ActorSource,
+ ActorSink,
+ WaypointFollower)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import generate_target_waypoint
+
+
+class SignalizedJunctionLeftTurn(BasicScenario):
+
+ """
+ Implementation class for Hero
+ Vehicle turning left at signalized junction scenario,
+ Traffic Scenario 08.
+
+ This is a single ego vehicle scenario
+ """
+
+ timeout = 80 # Timeout of scenario in seconds
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=80):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._world = world
+ self._map = CarlaDataProvider.get_map()
+ self._target_vel = 6.9
+ self._brake_value = 0.5
+ self._ego_distance = 110
+ self._traffic_light = None
+ self._other_actor_transform = None
+ self._blackboard_queue_name = 'SignalizedJunctionLeftTurn/actor_flow_queue'
+ self._queue = py_trees.blackboard.Blackboard().set(self._blackboard_queue_name, Queue())
+ self._initialized = True
+ super(SignalizedJunctionLeftTurn, self).__init__("TurnLeftAtSignalizedJunction",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ self._traffic_light = CarlaDataProvider.get_next_traffic_light(self.ego_vehicles[0], False)
+ traffic_light_other = CarlaDataProvider.get_next_traffic_light(self.other_actors[0], False)
+ if self._traffic_light is None or traffic_light_other is None:
+ raise RuntimeError("No traffic light for the given location found")
+ self._traffic_light.set_state(carla.TrafficLightState.Green)
+ self._traffic_light.set_green_time(self.timeout)
+ # other vehicle's traffic light
+ traffic_light_other.set_state(carla.TrafficLightState.Green)
+ traffic_light_other.set_green_time(self.timeout)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ self._other_actor_transform = config.other_actors[0].transform
+ first_vehicle_transform = carla.Transform(
+ carla.Location(config.other_actors[0].transform.location.x,
+ config.other_actors[0].transform.location.y,
+ config.other_actors[0].transform.location.z - 500),
+ config.other_actors[0].transform.rotation)
+ first_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, self._other_actor_transform)
+ first_vehicle.set_transform(first_vehicle_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ Hero vehicle is turning left in an urban area,
+ at a signalized intersection, while other actor coming straight
+ .The hero actor may turn left either before other actor
+ passes intersection or later, without any collision.
+ After 80 seconds, a timeout stops the scenario.
+ """
+
+ sequence = py_trees.composites.Sequence("Sequence Behavior")
+
+ # Selecting straight path at intersection
+ target_waypoint = generate_target_waypoint(
+ CarlaDataProvider.get_map().get_waypoint(self.other_actors[0].get_location()), 0)
+ # Generating waypoint list till next intersection
+ plan = []
+ wp_choice = target_waypoint.next(1.0)
+ while not wp_choice[0].is_intersection:
+ target_waypoint = wp_choice[0]
+ plan.append((target_waypoint, RoadOption.LANEFOLLOW))
+ wp_choice = target_waypoint.next(1.0)
+ # adding flow of actors
+ actor_source = ActorSource(
+ ['vehicle.tesla.model3', 'vehicle.audi.tt'],
+ self._other_actor_transform, 15, self._blackboard_queue_name)
+ # destroying flow of actors
+ actor_sink = ActorSink(plan[-1][0].transform.location, 10)
+ # follow waypoints untill next intersection
+ move_actor = WaypointFollower(self.other_actors[0], self._target_vel, plan=plan,
+ blackboard_queue_name=self._blackboard_queue_name, avoid_collision=True)
+ # wait
+ wait = DriveDistance(self.ego_vehicles[0], self._ego_distance)
+
+ # Behavior tree
+ root = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ root.add_child(wait)
+ root.add_child(actor_source)
+ root.add_child(actor_sink)
+ root.add_child(move_actor)
+
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform))
+ sequence.add_child(root)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collison_criteria = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collison_criteria)
+
+ return criteria
+
+ def __del__(self):
+ self._traffic_light = None
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/scenarios/signalized_junction_right_turn.py b/scenario_runner/srunner/scenarios/signalized_junction_right_turn.py
new file mode 100644
index 0000000..5ea7e2c
--- /dev/null
+++ b/scenario_runner/srunner/scenarios/signalized_junction_right_turn.py
@@ -0,0 +1,162 @@
+#!/usr/bin/env python
+
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Collection of traffic scenarios where the ego vehicle (hero)
+is making a right turn
+"""
+
+from __future__ import print_function
+
+import sys
+
+import py_trees
+
+import carla
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (ActorTransformSetter,
+ ActorDestroy,
+ StopVehicle,
+ SyncArrival,
+ WaypointFollower)
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import CollisionTest
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import DriveDistance, InTriggerDistanceToLocation
+from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.tools.scenario_helper import (get_geometric_linear_intersection,
+ get_crossing_point,
+ generate_target_waypoint)
+
+
+class SignalizedJunctionRightTurn(BasicScenario):
+
+ """
+ Implementation class for Hero
+ Vehicle turning right at signalized junction scenario,
+ Traffic Scenario 09.
+
+ This is a single ego vehicle scenario
+ """
+
+ def __init__(self, world, ego_vehicles, config, randomize=False, debug_mode=False, criteria_enable=True,
+ timeout=80):
+ """
+ Setup all relevant parameters and create scenario
+ """
+ self._target_vel = 6.9
+ self._brake_value = 0.5
+ self._ego_distance = 40
+ self._traffic_light = None
+ self._other_actor_transform = None
+ # Timeout of scenario in seconds
+ self.timeout = timeout
+ super(SignalizedJunctionRightTurn, self).__init__("HeroActorTurningRightAtSignalizedJunction",
+ ego_vehicles,
+ config,
+ world,
+ debug_mode,
+ criteria_enable=criteria_enable)
+
+ self._traffic_light = CarlaDataProvider.get_next_traffic_light(self.ego_vehicles[0], False)
+ if self._traffic_light is None:
+ print("No traffic light for the given location of the ego vehicle found")
+ sys.exit(-1)
+ self._traffic_light.set_state(carla.TrafficLightState.Red)
+ self._traffic_light.set_red_time(self.timeout)
+ # other vehicle's traffic light
+ traffic_light_other = CarlaDataProvider.get_next_traffic_light(self.other_actors[0], False)
+ if traffic_light_other is None:
+ print("No traffic light for the given location of the other vehicle found")
+ sys.exit(-1)
+ traffic_light_other.set_state(carla.TrafficLightState.Green)
+ traffic_light_other.set_green_time(self.timeout)
+
+ def _initialize_actors(self, config):
+ """
+ Custom initialization
+ """
+ self._other_actor_transform = config.other_actors[0].transform
+ first_vehicle_transform = carla.Transform(
+ carla.Location(config.other_actors[0].transform.location.x,
+ config.other_actors[0].transform.location.y,
+ config.other_actors[0].transform.location.z - 500),
+ config.other_actors[0].transform.rotation)
+ first_vehicle = CarlaDataProvider.request_new_actor(config.other_actors[0].model, first_vehicle_transform)
+ first_vehicle.set_simulate_physics(enabled=False)
+ self.other_actors.append(first_vehicle)
+
+ def _create_behavior(self):
+ """
+ Hero vehicle is turning right in an urban area,
+ at a signalized intersection, while other actor coming straight
+ from left.The hero actor may turn right either before other actor
+ passes intersection or later, without any collision.
+ After 80 seconds, a timeout stops the scenario.
+ """
+
+ location_of_collision_dynamic = get_geometric_linear_intersection(self.ego_vehicles[0], self.other_actors[0])
+ crossing_point_dynamic = get_crossing_point(self.other_actors[0])
+ sync_arrival = SyncArrival(
+ self.other_actors[0], self.ego_vehicles[0], location_of_collision_dynamic)
+ sync_arrival_stop = InTriggerDistanceToLocation(self.other_actors[0], crossing_point_dynamic, 5)
+
+ sync_arrival_parallel = py_trees.composites.Parallel(
+ "Synchronize arrival times",
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ sync_arrival_parallel.add_child(sync_arrival)
+ sync_arrival_parallel.add_child(sync_arrival_stop)
+
+ # Selecting straight path at intersection
+ target_waypoint = generate_target_waypoint(
+ CarlaDataProvider.get_map().get_waypoint(self.other_actors[0].get_location()), 0)
+ # Generating waypoint list till next intersection
+ plan = []
+ wp_choice = target_waypoint.next(1.0)
+ while not wp_choice[0].is_intersection:
+ target_waypoint = wp_choice[0]
+ plan.append((target_waypoint, RoadOption.LANEFOLLOW))
+ wp_choice = target_waypoint.next(1.0)
+
+ move_actor = WaypointFollower(self.other_actors[0], self._target_vel, plan=plan)
+ waypoint_follower_end = InTriggerDistanceToLocation(
+ self.other_actors[0], plan[-1][0].transform.location, 10)
+
+ move_actor_parallel = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
+ move_actor_parallel.add_child(move_actor)
+ move_actor_parallel.add_child(waypoint_follower_end)
+ # stop other actor
+ stop = StopVehicle(self.other_actors[0], self._brake_value)
+ # end condition
+ end_condition = DriveDistance(self.ego_vehicles[0], self._ego_distance)
+
+ # Behavior tree
+ sequence = py_trees.composites.Sequence()
+ sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_actor_transform))
+ sequence.add_child(sync_arrival_parallel)
+ sequence.add_child(move_actor_parallel)
+ sequence.add_child(stop)
+ sequence.add_child(end_condition)
+ sequence.add_child(ActorDestroy(self.other_actors[0]))
+
+ return sequence
+
+ def _create_test_criteria(self):
+ """
+ A list of all test criteria will be created that is later used
+ in parallel behavior tree.
+ """
+ criteria = []
+
+ collison_criteria = CollisionTest(self.ego_vehicles[0])
+ criteria.append(collison_criteria)
+
+ return criteria
+
+ def __del__(self):
+ self._traffic_light = None
+ self.remove_all_actors()
diff --git a/scenario_runner/srunner/tools/__init__.py b/scenario_runner/srunner/tools/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/scenario_runner/srunner/tools/openscenario_parser.py b/scenario_runner/srunner/tools/openscenario_parser.py
new file mode 100644
index 0000000..6b05cc4
--- /dev/null
+++ b/scenario_runner/srunner/tools/openscenario_parser.py
@@ -0,0 +1,1061 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019-2020 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides a parser for scenario configuration files based on OpenSCENARIO
+"""
+
+from distutils.util import strtobool
+import copy
+import datetime
+import math
+import operator
+
+import py_trees
+import carla
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.weather_sim import Weather
+from srunner.scenariomanager.scenarioatomics.atomic_behaviors import (TrafficLightStateSetter,
+ ActorTransformSetterToOSCPosition,
+ RunScript,
+ ChangeWeather,
+ ChangeAutoPilot,
+ ChangeRoadFriction,
+ ChangeActorTargetSpeed,
+ ChangeActorControl,
+ ChangeActorWaypoints,
+ ChangeActorWaypointsToReachPosition,
+ ChangeActorLateralMotion,
+ Idle)
+# pylint: disable=unused-import
+# For the following includes the pylint check is disabled, as these are accessed via globals()
+from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest,
+ MaxVelocityTest,
+ DrivenDistanceTest,
+ AverageVelocityTest,
+ KeepLaneTest,
+ ReachedRegionTest,
+ OnSidewalkTest,
+ WrongLaneTest,
+ InRadiusRegionTest,
+ InRouteTest,
+ RouteCompletionTest,
+ RunningRedLightTest,
+ RunningStopTest,
+ OffRoadTest,
+ EndofRoadTest)
+# pylint: enable=unused-import
+from srunner.scenariomanager.scenarioatomics.atomic_trigger_conditions import (InTriggerDistanceToVehicle,
+ InTriggerDistanceToOSCPosition,
+ InTimeToArrivalToOSCPosition,
+ InTimeToArrivalToVehicle,
+ DriveDistance,
+ StandStill,
+ OSCStartEndCondition,
+ TriggerAcceleration,
+ RelativeVelocityToOtherActor,
+ TimeOfDayComparison,
+ TriggerVelocity,
+ WaitForTrafficLightState)
+from srunner.scenariomanager.timer import TimeOut, SimulationTimeCondition
+from srunner.tools.py_trees_port import oneshot_behavior
+
+
+class OpenScenarioParser(object):
+
+ """
+ Pure static class providing conversions from OpenSCENARIO elements to ScenarioRunner elements
+ """
+ operators = {
+ "greaterThan": operator.gt,
+ "lessThan": operator.lt,
+ "equalTo": operator.eq
+ }
+
+ actor_types = {
+ "pedestrian": "walker",
+ "vehicle": "vehicle",
+ "miscellaneous": "miscellaneous"
+ }
+
+ tl_states = {
+ "GREEN": carla.TrafficLightState.Green,
+ "YELLOW": carla.TrafficLightState.Yellow,
+ "RED": carla.TrafficLightState.Red,
+ "OFF": carla.TrafficLightState.Off,
+ }
+
+ global_osc_parameters = dict()
+ use_carla_coordinate_system = False
+ osc_filepath = None
+
+ @staticmethod
+ def get_traffic_light_from_osc_name(name):
+ """
+ Returns a carla.TrafficLight instance that matches the name given
+ """
+ traffic_light = None
+
+ # Given by id
+ if name.startswith("id="):
+ tl_id = name[3:]
+ for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'):
+ if carla_tl.id == tl_id:
+ traffic_light = carla_tl
+ break
+ # Given by position
+ elif name.startswith("pos="):
+ tl_pos = name[4:]
+ pos = tl_pos.split(",")
+ for carla_tl in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'):
+ carla_tl_location = carla_tl.get_transform().location
+ distance = carla_tl_location.distance(carla.Location(float(pos[0]),
+ float(pos[1]),
+ carla_tl_location.z))
+ if distance < 2.0:
+ traffic_light = carla_tl
+ break
+
+ if traffic_light is None:
+ raise AttributeError("Unknown traffic light {}".format(name))
+
+ return traffic_light
+
+ @staticmethod
+ def set_osc_filepath(filepath):
+ """
+ Set path of OSC file. This is required if for example custom commands are provided with
+ relative paths.
+ """
+ OpenScenarioParser.osc_filepath = filepath
+
+ @staticmethod
+ def set_use_carla_coordinate_system():
+ """
+ CARLA internally uses a left-hand coordinate system (Unreal), but OpenSCENARIO and OpenDRIVE
+ are intended for right-hand coordinate system. Hence, we need to invert the coordinates, if
+ the scenario does not use CARLA coordinates, but instead right-hand coordinates.
+ """
+ OpenScenarioParser.use_carla_coordinate_system = True
+
+ @staticmethod
+ def set_parameters(xml_tree, additional_parameter_dict=None):
+ """
+ Parse the xml_tree, and replace all parameter references
+ with the actual values.
+
+ Note: Parameter names must not start with "$", however when referencing a parameter the
+ reference has to start with "$".
+ https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html#_re_use_mechanisms
+
+ Args:
+ xml_tree: Containing all nodes that should be updated
+ additional_parameter_dict (dictionary): Additional parameters as dict (key, value). Optional.
+
+ returns:
+ updated xml_tree, dictonary containing all parameters and their values
+ """
+
+ parameter_dict = dict()
+ if additional_parameter_dict is not None:
+ parameter_dict = additional_parameter_dict
+ parameters = xml_tree.find('ParameterDeclarations')
+
+ if parameters is None and not parameter_dict:
+ return xml_tree, parameter_dict
+
+ if parameters is None:
+ parameters = []
+
+ for parameter in parameters:
+ name = parameter.attrib.get('name')
+ value = parameter.attrib.get('value')
+ parameter_dict[name] = value
+
+ for node in xml_tree.iter():
+ for key in node.attrib:
+ for param in sorted(parameter_dict, key=len, reverse=True):
+ if "$" + param in node.attrib[key]:
+ node.attrib[key] = node.attrib[key].replace("$" + param, parameter_dict[param])
+
+ return xml_tree, parameter_dict
+
+ @staticmethod
+ def set_global_parameters(parameter_dict):
+ """
+ Set global_osc_parameter dictionary
+
+ Args:
+ parameter_dict (Dictionary): Input for global_osc_parameter
+ """
+ OpenScenarioParser.global_osc_parameters = parameter_dict
+
+ @staticmethod
+ def get_catalog_entry(catalogs, catalog_reference):
+ """
+ Get catalog entry referenced by catalog_reference included correct parameter settings
+
+ Args:
+ catalogs (Dictionary of dictionaries): List of all catalogs and their entries
+ catalog_reference (XML ElementTree): Reference containing the exact catalog to be used
+
+ returns:
+ Catalog entry (XML ElementTree)
+ """
+
+ entry = catalogs[catalog_reference.attrib.get("catalogName")][catalog_reference.attrib.get("entryName")]
+ entry_copy = copy.deepcopy(entry)
+ catalog_copy = copy.deepcopy(catalog_reference)
+ entry = OpenScenarioParser.assign_catalog_parameters(entry_copy, catalog_copy)
+
+ return entry
+
+ @staticmethod
+ def assign_catalog_parameters(entry_instance, catalog_reference):
+ """
+ Parse catalog_reference, and replace all parameter references
+ in entry_instance by the values provided in catalog_reference.
+
+ Not to be used from outside this class.
+
+ Args:
+ entry_instance (XML ElementTree): Entry to be updated
+ catalog_reference (XML ElementTree): Reference containing the exact parameter values
+
+ returns:
+ updated entry_instance with updated parameter values
+ """
+
+ parameter_dict = dict()
+ for elem in entry_instance.iter():
+ if elem.find('ParameterDeclarations') is not None:
+ parameters = elem.find('ParameterDeclarations')
+ for parameter in parameters:
+ name = parameter.attrib.get('name')
+ value = parameter.attrib.get('value')
+ parameter_dict[name] = value
+
+ for parameter_assignments in catalog_reference.iter("ParameterAssignments"):
+ for parameter_assignment in parameter_assignments.iter("ParameterAssignment"):
+ parameter = parameter_assignment.attrib.get("parameterRef")
+ value = parameter_assignment.attrib.get("value")
+ parameter_dict[parameter] = value
+
+ for node in entry_instance.iter():
+ for key in node.attrib:
+ for param in sorted(parameter_dict, key=len, reverse=True):
+ if "$" + param in node.attrib[key]:
+ node.attrib[key] = node.attrib[key].replace("$" + param, parameter_dict[param])
+
+ OpenScenarioParser.set_parameters(entry_instance, OpenScenarioParser.global_osc_parameters)
+
+ return entry_instance
+
+ @staticmethod
+ def get_friction_from_env_action(xml_tree, catalogs):
+ """
+ Extract the CARLA road friction coefficient from an OSC EnvironmentAction
+
+ Args:
+ xml_tree: Containing the EnvironmentAction,
+ or the reference to the catalog it is defined in.
+ catalogs: XML Catalogs that could contain the EnvironmentAction
+
+ returns:
+ friction (float)
+ """
+ set_environment = next(xml_tree.iter("EnvironmentAction"))
+
+ if sum(1 for _ in set_environment.iter("Weather")) != 0:
+ environment = set_environment.find("Environment")
+ elif set_environment.find("CatalogReference") is not None:
+ catalog_reference = set_environment.find("CatalogReference")
+ environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference)
+
+ friction = 1.0
+
+ road_condition = environment.iter("RoadCondition")
+ for condition in road_condition:
+ friction = condition.attrib.get('frictionScaleFactor')
+
+ return friction
+
+ @staticmethod
+ def get_weather_from_env_action(xml_tree, catalogs):
+ """
+ Extract the CARLA weather parameters from an OSC EnvironmentAction
+
+ Args:
+ xml_tree: Containing the EnvironmentAction,
+ or the reference to the catalog it is defined in.
+ catalogs: XML Catalogs that could contain the EnvironmentAction
+
+ returns:
+ Weather (srunner.scenariomanager.weather_sim.Weather)
+ """
+ set_environment = next(xml_tree.iter("EnvironmentAction"))
+
+ if sum(1 for _ in set_environment.iter("Weather")) != 0:
+ environment = set_environment.find("Environment")
+ elif set_environment.find("CatalogReference") is not None:
+ catalog_reference = set_environment.find("CatalogReference")
+ environment = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference)
+
+ weather = environment.find("Weather")
+ sun = weather.find("Sun")
+
+ carla_weather = carla.WeatherParameters()
+ carla_weather.sun_azimuth_angle = math.degrees(float(sun.attrib.get('azimuth', 0)))
+ carla_weather.sun_altitude_angle = math.degrees(float(sun.attrib.get('elevation', 0)))
+ carla_weather.cloudiness = 100 - float(sun.attrib.get('intensity', 0)) * 100
+ fog = weather.find("Fog")
+ carla_weather.fog_distance = float(fog.attrib.get('visualRange', 'inf'))
+ if carla_weather.fog_distance < 1000:
+ carla_weather.fog_density = 100
+ carla_weather.precipitation = 0
+ carla_weather.precipitation_deposits = 0
+ carla_weather.wetness = 0
+ carla_weather.wind_intensity = 0
+ precepitation = weather.find("Precipitation")
+ if precepitation.attrib.get('precipitationType') == "rain":
+ carla_weather.precipitation = float(precepitation.attrib.get('intensity')) * 100
+ carla_weather.precipitation_deposits = 100 # if it rains, make the road wet
+ carla_weather.wetness = carla_weather.precipitation
+ elif precepitation.attrib.get('type') == "snow":
+ raise AttributeError("CARLA does not support snow precipitation")
+
+ time_of_day = environment.find("TimeOfDay")
+ weather_animation = strtobool(time_of_day.attrib.get("animation"))
+ time = time_of_day.attrib.get("dateTime")
+ dtime = datetime.datetime.strptime(time, "%Y-%m-%dT%H:%M:%S")
+
+ return Weather(carla_weather, dtime, weather_animation)
+
+ @staticmethod
+ def get_controller(xml_tree, catalogs):
+ """
+ Extract the object controller from the OSC XML or a catalog
+
+ Args:
+ xml_tree: Containing the controller information,
+ or the reference to the catalog it is defined in.
+ catalogs: XML Catalogs that could contain the EnvironmentAction
+
+ returns:
+ module: Python module containing the controller implementation
+ args: Dictonary with (key, value) parameters for the controller
+ """
+
+ assign_action = next(xml_tree.iter("AssignControllerAction"))
+
+ properties = None
+ if assign_action.find('Controller') is not None:
+ properties = assign_action.find('Controller').find('Properties')
+ elif assign_action.find("CatalogReference") is not None:
+ catalog_reference = assign_action.find("CatalogReference")
+ properties = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference).find('Properties')
+
+ module = None
+ args = {}
+ for prop in properties:
+ if prop.attrib.get('name') == "module":
+ module = prop.attrib.get('value')
+ else:
+ args[prop.attrib.get('name')] = prop.attrib.get('value')
+
+ override_action = xml_tree.find('OverrideControllerValueAction')
+ for child in override_action:
+ if strtobool(child.attrib.get('active')):
+ raise NotImplementedError("Controller override actions are not yet supported")
+
+ return module, args
+
+ @staticmethod
+ def get_route(xml_tree, catalogs):
+ """
+ Extract the route from the OSC XML or a catalog
+
+ Args:
+ xml_tree: Containing the route information,
+ or the reference to the catalog it is defined in.
+ catalogs: XML Catalogs that could contain the Route
+
+ returns:
+ waypoints: List of route waypoints
+ """
+ route = None
+
+ if xml_tree.find('Route') is not None:
+ route = xml_tree.find('Route')
+ elif xml_tree.find('CatalogReference') is not None:
+ catalog_reference = xml_tree.find("CatalogReference")
+ route = OpenScenarioParser.get_catalog_entry(catalogs, catalog_reference)
+ else:
+ raise AttributeError("Unknown private FollowRoute action")
+
+ waypoints = []
+ if route is not None:
+ for waypoint in route.iter('Waypoint'):
+ position = waypoint.find('Position')
+ transform = OpenScenarioParser.convert_position_to_transform(position)
+ waypoints.append(transform)
+
+ return waypoints
+
+ @staticmethod
+ def convert_position_to_transform(position, actor_list=None):
+ """
+ Convert an OpenScenario position into a CARLA transform
+
+ Not supported: Road, RelativeRoad, Lane, RelativeLane as the PythonAPI currently
+ does not provide sufficient access to OpenDrive information
+ Also not supported is Route. This can be added by checking additional
+ route information
+ """
+
+ if position.find('WorldPosition') is not None:
+ world_pos = position.find('WorldPosition')
+ x = float(world_pos.attrib.get('x', 0))
+ y = float(world_pos.attrib.get('y', 0))
+ z = float(world_pos.attrib.get('z', 0))
+ yaw = math.degrees(float(world_pos.attrib.get('h', 0)))
+ pitch = math.degrees(float(world_pos.attrib.get('p', 0)))
+ roll = math.degrees(float(world_pos.attrib.get('r', 0)))
+ if not OpenScenarioParser.use_carla_coordinate_system:
+ y = y * (-1.0)
+ yaw = yaw * (-1.0)
+ return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
+
+ elif ((position.find('RelativeWorldPosition') is not None) or
+ (position.find('RelativeObjectPosition') is not None) or
+ (position.find('RelativeLanePosition') is not None)):
+
+ if position.find('RelativeWorldPosition') is not None:
+ rel_pos = position.find('RelativeWorldPosition')
+ if position.find('RelativeObjectPosition') is not None:
+ rel_pos = position.find('RelativeObjectPosition')
+ if position.find('RelativeLanePosition') is not None:
+ rel_pos = position.find('RelativeLanePosition')
+
+ # get relative object and relative position
+ obj = rel_pos.attrib.get('entityRef')
+ obj_actor = None
+ actor_transform = None
+
+ if actor_list is not None:
+ for actor in actor_list:
+ if actor.rolename == obj:
+ obj_actor = actor
+ actor_transform = actor.transform
+ else:
+ for actor in CarlaDataProvider.get_world().get_actors():
+ if 'role_name' in actor.attributes and actor.attributes['role_name'] == obj:
+ obj_actor = actor
+ actor_transform = obj_actor.get_transform()
+ break
+
+ if obj_actor is None:
+ raise AttributeError("Object '{}' provided as position reference is not known".format(obj))
+
+ # calculate orientation h, p, r
+ is_absolute = False
+ dyaw = 0
+ dpitch = 0
+ droll = 0
+ if rel_pos.find('Orientation') is not None:
+ orientation = rel_pos.find('Orientation')
+ is_absolute = (orientation.attrib.get('type') == "absolute")
+ dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
+ dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
+ droll = math.degrees(float(orientation.attrib.get('r', 0)))
+
+ if not OpenScenarioParser.use_carla_coordinate_system:
+ dyaw = dyaw * (-1.0)
+
+ yaw = actor_transform.rotation.yaw
+ pitch = actor_transform.rotation.pitch
+ roll = actor_transform.rotation.roll
+
+ if not is_absolute:
+ yaw = yaw + dyaw
+ pitch = pitch + dpitch
+ roll = roll + droll
+ else:
+ yaw = dyaw
+ pitch = dpitch
+ roll = droll
+
+ # calculate location x, y, z
+ # dx, dy, dz
+ if ((position.find('RelativeWorldPosition') is not None) or
+ (position.find('RelativeObjectPosition') is not None)):
+ dx = float(rel_pos.attrib.get('dx', 0))
+ dy = float(rel_pos.attrib.get('dy', 0))
+ dz = float(rel_pos.attrib.get('dz', 0))
+
+ if not OpenScenarioParser.use_carla_coordinate_system:
+ dy = dy * (-1.0)
+
+ x = actor_transform.location.x + dx
+ y = actor_transform.location.y + dy
+ z = actor_transform.location.z + dz
+
+ # dLane, ds, offset
+ elif position.find('RelativeLanePosition') is not None:
+ dlane = float(rel_pos.attrib.get('dLane'))
+ ds = float(rel_pos.attrib.get('ds'))
+ offset = float(rel_pos.attrib.get('offset', 0.0))
+
+ carla_map = CarlaDataProvider.get_map()
+ relative_waypoint = carla_map.get_waypoint(actor_transform.location)
+
+ if dlane == 0:
+ wp = relative_waypoint
+ elif dlane == -1:
+ wp = relative_waypoint.get_left_lane()
+ elif dlane == 1:
+ wp = relative_waypoint.get_right_lane()
+ if wp is None:
+ raise AttributeError("Object '{}' position with dLane={} is not valid".format(obj, dlane))
+
+ if ds < 0:
+ ds = (-1.0) * ds
+ wp = wp.previous(ds)[-1]
+ else:
+ wp = wp.next(ds)[-1]
+
+ # Adapt transform according to offset
+ h = math.radians(wp.transform.rotation.yaw)
+ x_offset = math.sin(h) * offset
+ y_offset = math.cos(h) * offset
+
+ if OpenScenarioParser.use_carla_coordinate_system:
+ x_offset = x_offset * (-1.0)
+ y_offset = y_offset * (-1.0)
+
+ x = wp.transform.location.x + x_offset
+ y = wp.transform.location.y + y_offset
+ z = wp.transform.location.z
+
+ return carla.Transform(carla.Location(x=x, y=y, z=z), carla.Rotation(yaw=yaw, pitch=pitch, roll=roll))
+
+ # Not implemented
+ elif position.find('RoadPosition') is not None:
+ raise NotImplementedError("Road positions are not yet supported")
+ elif position.find('RelativeRoadPosition') is not None:
+ raise NotImplementedError("RelativeRoad positions are not yet supported")
+ elif position.find('LanePosition') is not None:
+ lane_pos = position.find('LanePosition')
+ road_id = int(lane_pos.attrib.get('roadId', 0))
+ lane_id = int(lane_pos.attrib.get('laneId', 0))
+ offset = float(lane_pos.attrib.get('offset', 0))
+ s = float(lane_pos.attrib.get('s', 0))
+ is_absolute = True
+ waypoint = CarlaDataProvider.get_map().get_waypoint_xodr(road_id, lane_id, s)
+ if waypoint is None:
+ raise AttributeError("Lane position cannot be found")
+
+ transform = waypoint.transform
+ if lane_pos.find('Orientation') is not None:
+ orientation = lane_pos.find('Orientation')
+ dyaw = math.degrees(float(orientation.attrib.get('h', 0)))
+ dpitch = math.degrees(float(orientation.attrib.get('p', 0)))
+ droll = math.degrees(float(orientation.attrib.get('r', 0)))
+
+ if not OpenScenarioParser.use_carla_coordinate_system:
+ dyaw = dyaw * (-1.0)
+
+ transform.rotation.yaw = transform.rotation.yaw + dyaw
+ transform.rotation.pitch = transform.rotation.pitch + dpitch
+ transform.rotation.roll = transform.rotation.roll + droll
+
+ if offset != 0:
+ forward_vector = transform.rotation.get_forward_vector()
+ orthogonal_vector = carla.Vector3D(x=-forward_vector.y, y=forward_vector.x, z=forward_vector.z)
+ transform.location.x = transform.location.x + offset * orthogonal_vector.x
+ transform.location.y = transform.location.y + offset * orthogonal_vector.y
+
+ return transform
+ elif position.find('RoutePosition') is not None:
+ raise NotImplementedError("Route positions are not yet supported")
+ else:
+ raise AttributeError("Unknown position")
+
+ @staticmethod
+ def convert_condition_to_atomic(condition, actor_list):
+ """
+ Convert an OpenSCENARIO condition into a Behavior/Criterion atomic
+
+ If there is a delay defined in the condition, then the condition is checked after the delay time
+ passed by, e.g. .
+
+ Note: Not all conditions are currently supported.
+ """
+
+ atomic = None
+ delay_atomic = None
+ condition_name = condition.attrib.get('name')
+
+ if condition.attrib.get('delay') is not None and str(condition.attrib.get('delay')) != '0':
+ delay = float(condition.attrib.get('delay'))
+ delay_atomic = TimeOut(delay)
+
+ if condition.find('ByEntityCondition') is not None:
+
+ trigger_actor = None # A-priori validation ensures that this will be not None
+ triggered_actor = None
+
+ for triggering_entities in condition.find('ByEntityCondition').iter('TriggeringEntities'):
+ for entity in triggering_entities.iter('EntityRef'):
+ for actor in actor_list:
+ if entity.attrib.get('entityRef', None) == actor.attributes['role_name']:
+ trigger_actor = actor
+ break
+
+ for entity_condition in condition.find('ByEntityCondition').iter('EntityCondition'):
+ if entity_condition.find('EndOfRoadCondition') is not None:
+ end_road_condition = entity_condition.find('EndOfRoadCondition')
+ condition_duration = float(end_road_condition.attrib.get('duration'))
+ atomic_cls = py_trees.meta.inverter(EndofRoadTest)
+ atomic = atomic_cls(
+ trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name)
+ elif entity_condition.find('CollisionCondition') is not None:
+
+ collision_condition = entity_condition.find('CollisionCondition')
+
+ if collision_condition.find('EntityRef') is not None:
+ collision_entity = collision_condition.find('EntityRef')
+
+ for actor in actor_list:
+ if collision_entity.attrib.get('entityRef', None) == actor.attributes['role_name']:
+ triggered_actor = actor
+ break
+
+ if triggered_actor is None:
+ raise AttributeError("Cannot find actor '{}' for condition".format(
+ collision_condition.attrib.get('entityRef', None)))
+
+ atomic_cls = py_trees.meta.inverter(CollisionTest)
+ atomic = atomic_cls(trigger_actor, other_actor=triggered_actor,
+ terminate_on_failure=True, name=condition_name)
+
+ elif collision_condition.find('ByType') is not None:
+ collision_type = collision_condition.find('ByType').attrib.get('type', None)
+
+ triggered_type = OpenScenarioParser.actor_types[collision_type]
+
+ atomic_cls = py_trees.meta.inverter(CollisionTest)
+ atomic = atomic_cls(trigger_actor, other_actor_type=triggered_type,
+ terminate_on_failure=True, name=condition_name)
+
+ else:
+ atomic_cls = py_trees.meta.inverter(CollisionTest)
+ atomic = atomic_cls(trigger_actor, terminate_on_failure=True, name=condition_name)
+
+ elif entity_condition.find('OffroadCondition') is not None:
+ off_condition = entity_condition.find('OffroadCondition')
+ condition_duration = float(off_condition.attrib.get('duration'))
+ atomic_cls = py_trees.meta.inverter(OffRoadTest)
+ atomic = atomic_cls(
+ trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name)
+ elif entity_condition.find('TimeHeadwayCondition') is not None:
+ headtime_condition = entity_condition.find('TimeHeadwayCondition')
+
+ condition_value = float(headtime_condition.attrib.get('value'))
+
+ condition_rule = headtime_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+
+ condition_freespace = strtobool(headtime_condition.attrib.get('freespace', False))
+ if condition_freespace:
+ raise NotImplementedError(
+ "TimeHeadwayCondition: freespace attribute is currently not implemented")
+ condition_along_route = strtobool(headtime_condition.attrib.get('alongRoute', False))
+
+ for actor in actor_list:
+ if headtime_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
+ triggered_actor = actor
+ break
+ if triggered_actor is None:
+ raise AttributeError("Cannot find actor '{}' for condition".format(
+ headtime_condition.attrib.get('entityRef', None)))
+
+ atomic = InTimeToArrivalToVehicle(
+ trigger_actor, triggered_actor, condition_value,
+ condition_along_route, condition_operator, condition_name
+ )
+
+ elif entity_condition.find('TimeToCollisionCondition') is not None:
+ ttc_condition = entity_condition.find('TimeToCollisionCondition')
+
+ condition_rule = ttc_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+
+ condition_value = ttc_condition.attrib.get('value')
+ condition_target = ttc_condition.find('TimeToCollisionConditionTarget')
+
+ condition_freespace = strtobool(ttc_condition.attrib.get('freespace', False))
+ if condition_freespace:
+ raise NotImplementedError(
+ "TimeToCollisionCondition: freespace attribute is currently not implemented")
+ condition_along_route = strtobool(ttc_condition.attrib.get('alongRoute', False))
+
+ if condition_target.find('Position') is not None:
+ position = condition_target.find('Position')
+ atomic = InTimeToArrivalToOSCPosition(
+ trigger_actor, position, condition_value, condition_along_route, condition_operator)
+ else:
+ for actor in actor_list:
+ if ttc_condition.attrib.get('EntityRef', None) == actor.attributes['role_name']:
+ triggered_actor = actor
+ break
+ if triggered_actor is None:
+ raise AttributeError("Cannot find actor '{}' for condition".format(
+ ttc_condition.attrib.get('EntityRef', None)))
+
+ atomic = InTimeToArrivalToVehicle(
+ trigger_actor, triggered_actor, condition_value,
+ condition_along_route, condition_operator, condition_name)
+ elif entity_condition.find('AccelerationCondition') is not None:
+ accel_condition = entity_condition.find('AccelerationCondition')
+ condition_value = float(accel_condition.attrib.get('value'))
+ condition_rule = accel_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+ atomic = TriggerAcceleration(
+ trigger_actor, condition_value, condition_operator, condition_name)
+ elif entity_condition.find('StandStillCondition') is not None:
+ ss_condition = entity_condition.find('StandStillCondition')
+ duration = float(ss_condition.attrib.get('duration'))
+ atomic = StandStill(trigger_actor, condition_name, duration)
+ elif entity_condition.find('SpeedCondition') is not None:
+ spd_condition = entity_condition.find('SpeedCondition')
+ condition_value = float(spd_condition.attrib.get('value'))
+ condition_rule = spd_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+
+ atomic = TriggerVelocity(
+ trigger_actor, condition_value, condition_operator, condition_name)
+ elif entity_condition.find('RelativeSpeedCondition') is not None:
+ relspd_condition = entity_condition.find('RelativeSpeedCondition')
+ condition_value = float(relspd_condition.attrib.get('value'))
+ condition_rule = relspd_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+
+ for actor in actor_list:
+ if relspd_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
+ triggered_actor = actor
+ break
+
+ if triggered_actor is None:
+ raise AttributeError("Cannot find actor '{}' for condition".format(
+ relspd_condition.attrib.get('entityRef', None)))
+
+ atomic = RelativeVelocityToOtherActor(
+ trigger_actor, triggered_actor, condition_value, condition_operator, condition_name)
+ elif entity_condition.find('TraveledDistanceCondition') is not None:
+ distance_condition = entity_condition.find('TraveledDistanceCondition')
+ distance_value = float(distance_condition.attrib.get('value'))
+ atomic = DriveDistance(trigger_actor, distance_value, name=condition_name)
+ elif entity_condition.find('ReachPositionCondition') is not None:
+ rp_condition = entity_condition.find('ReachPositionCondition')
+ distance_value = float(rp_condition.attrib.get('tolerance'))
+ position = rp_condition.find('Position')
+ atomic = InTriggerDistanceToOSCPosition(
+ trigger_actor, position, distance_value, name=condition_name)
+ elif entity_condition.find('DistanceCondition') is not None:
+ distance_condition = entity_condition.find('DistanceCondition')
+
+ distance_value = float(distance_condition.attrib.get('value'))
+
+ distance_rule = distance_condition.attrib.get('rule')
+ distance_operator = OpenScenarioParser.operators[distance_rule]
+
+ distance_freespace = strtobool(distance_condition.attrib.get('freespace', False))
+ if distance_freespace:
+ raise NotImplementedError(
+ "DistanceCondition: freespace attribute is currently not implemented")
+ distance_along_route = strtobool(distance_condition.attrib.get('alongRoute', False))
+
+ if distance_condition.find('Position') is not None:
+ position = distance_condition.find('Position')
+ atomic = InTriggerDistanceToOSCPosition(
+ trigger_actor, position, distance_value, distance_along_route,
+ distance_operator, name=condition_name)
+
+ elif entity_condition.find('RelativeDistanceCondition') is not None:
+ distance_condition = entity_condition.find('RelativeDistanceCondition')
+ distance_value = float(distance_condition.attrib.get('value'))
+
+ distance_freespace = strtobool(distance_condition.attrib.get('freespace', False))
+ if distance_freespace:
+ raise NotImplementedError(
+ "RelativeDistanceCondition: freespace attribute is currently not implemented")
+ if distance_condition.attrib.get('relativeDistanceType') == "cartesianDistance":
+ for actor in actor_list:
+ if distance_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
+ triggered_actor = actor
+ break
+
+ if triggered_actor is None:
+ raise AttributeError("Cannot find actor '{}' for condition".format(
+ distance_condition.attrib.get('entityRef', None)))
+
+ condition_rule = distance_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+ atomic = InTriggerDistanceToVehicle(
+ triggered_actor, trigger_actor, distance_value, condition_operator, name=condition_name)
+ else:
+ raise NotImplementedError(
+ "RelativeDistance condition with the given specification is not yet supported")
+ elif condition.find('ByValueCondition') is not None:
+ value_condition = condition.find('ByValueCondition')
+ if value_condition.find('ParameterCondition') is not None:
+ parameter_condition = value_condition.find('ParameterCondition')
+ arg_name = parameter_condition.attrib.get('parameterRef')
+ value = parameter_condition.attrib.get('value')
+ if value != '':
+ arg_value = float(value)
+ else:
+ arg_value = 0
+ parameter_condition.attrib.get('rule')
+
+ if condition_name in globals():
+ criterion_instance = globals()[condition_name]
+ else:
+ raise AttributeError(
+ "The condition {} cannot be mapped to a criterion atomic".format(condition_name))
+
+ atomic = py_trees.composites.Parallel("Evaluation Criteria for multiple ego vehicles")
+ for triggered_actor in actor_list:
+ if arg_name != '':
+ atomic.add_child(criterion_instance(triggered_actor, arg_value))
+ else:
+ atomic.add_child(criterion_instance(triggered_actor))
+ elif value_condition.find('SimulationTimeCondition') is not None:
+ simtime_condition = value_condition.find('SimulationTimeCondition')
+ value = float(simtime_condition.attrib.get('value'))
+ rule = simtime_condition.attrib.get('rule')
+ atomic = SimulationTimeCondition(value, success_rule=rule)
+ elif value_condition.find('TimeOfDayCondition') is not None:
+ tod_condition = value_condition.find('TimeOfDayCondition')
+ condition_date = tod_condition.attrib.get('dateTime')
+ condition_rule = tod_condition.attrib.get('rule')
+ condition_operator = OpenScenarioParser.operators[condition_rule]
+ atomic = TimeOfDayComparison(condition_date, condition_operator, condition_name)
+ elif value_condition.find('StoryboardElementStateCondition') is not None:
+ state_condition = value_condition.find('StoryboardElementStateCondition')
+ element_name = state_condition.attrib.get('storyboardElementRef')
+ element_type = state_condition.attrib.get('storyboardElementType')
+ state = state_condition.attrib.get('state')
+ if state == "startTransition":
+ atomic = OSCStartEndCondition(element_type, element_name, rule="START", name=state + "Condition")
+ elif state == "stopTransition" or state == "endTransition" or state == "completeState":
+ atomic = OSCStartEndCondition(element_type, element_name, rule="END", name=state + "Condition")
+ else:
+ raise NotImplementedError(
+ "Only start, stop, endTransitions and completeState are currently supported")
+ elif value_condition.find('UserDefinedValueCondition') is not None:
+ raise NotImplementedError("ByValue UserDefinedValue conditions are not yet supported")
+ elif value_condition.find('TrafficSignalCondition') is not None:
+ tl_condition = value_condition.find('TrafficSignalCondition')
+
+ name_condition = tl_condition.attrib.get('name')
+ traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition)
+
+ tl_state = tl_condition.attrib.get('state').upper()
+ if tl_state not in OpenScenarioParser.tl_states:
+ raise KeyError("CARLA only supports Green, Red, Yellow or Off")
+ state_condition = OpenScenarioParser.tl_states[tl_state]
+
+ atomic = WaitForTrafficLightState(
+ traffic_light, state_condition, name=condition_name)
+ elif value_condition.find('TrafficSignalControllerCondition') is not None:
+ raise NotImplementedError("ByValue TrafficSignalController conditions are not yet supported")
+ else:
+ raise AttributeError("Unknown ByValue condition")
+
+ else:
+ raise AttributeError("Unknown condition")
+
+ if delay_atomic is not None and atomic is not None:
+ new_atomic = py_trees.composites.Sequence("delayed sequence")
+ new_atomic.add_child(delay_atomic)
+ new_atomic.add_child(atomic)
+ else:
+ new_atomic = atomic
+
+ return new_atomic
+
+ @staticmethod
+ def convert_maneuver_to_atomic(action, actor, catalogs):
+ """
+ Convert an OpenSCENARIO maneuver action into a Behavior atomic
+
+ Note not all OpenSCENARIO actions are currently supported
+ """
+ maneuver_name = action.attrib.get('name', 'unknown')
+
+ if action.find('GlobalAction') is not None:
+ global_action = action.find('GlobalAction')
+ if global_action.find('InfrastructureAction') is not None:
+ infrastructure_action = global_action.find('InfrastructureAction').find('TrafficSignalAction')
+ if infrastructure_action.find('TrafficSignalStateAction') is not None:
+ traffic_light_action = infrastructure_action.find('TrafficSignalStateAction')
+
+ name_condition = traffic_light_action.attrib.get('name')
+ traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition)
+
+ tl_state = traffic_light_action.attrib.get('state').upper()
+ if tl_state not in OpenScenarioParser.tl_states:
+ raise KeyError("CARLA only supports Green, Red, Yellow or Off")
+ traffic_light_state = OpenScenarioParser.tl_states[tl_state]
+
+ atomic = TrafficLightStateSetter(
+ traffic_light, traffic_light_state, name=maneuver_name + "_" + str(traffic_light.id))
+ else:
+ raise NotImplementedError("TrafficLights can only be influenced via TrafficSignalStateAction")
+ elif global_action.find('EnvironmentAction') is not None:
+ weather_behavior = ChangeWeather(
+ OpenScenarioParser.get_weather_from_env_action(global_action, catalogs))
+ friction_behavior = ChangeRoadFriction(
+ OpenScenarioParser.get_friction_from_env_action(global_action, catalogs))
+
+ env_behavior = py_trees.composites.Parallel(
+ policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=maneuver_name)
+
+ env_behavior.add_child(
+ oneshot_behavior(variable_name=maneuver_name + ">WeatherUpdate", behaviour=weather_behavior))
+ env_behavior.add_child(
+ oneshot_behavior(variable_name=maneuver_name + ">FrictionUpdate", behaviour=friction_behavior))
+
+ return env_behavior
+
+ else:
+ raise NotImplementedError("Global actions are not yet supported")
+ elif action.find('UserDefinedAction') is not None:
+ user_defined_action = action.find('UserDefinedAction')
+ if user_defined_action.find('CustomCommandAction') is not None:
+ command = user_defined_action.find('CustomCommandAction').attrib.get('type')
+ atomic = RunScript(command, base_path=OpenScenarioParser.osc_filepath, name=maneuver_name)
+ elif action.find('PrivateAction') is not None:
+ private_action = action.find('PrivateAction')
+
+ if private_action.find('LongitudinalAction') is not None:
+ private_action = private_action.find('LongitudinalAction')
+
+ if private_action.find('SpeedAction') is not None:
+ long_maneuver = private_action.find('SpeedAction')
+
+ # duration and distance
+ distance = float('inf')
+ duration = float('inf')
+ dimension = long_maneuver.find("SpeedActionDynamics").attrib.get('dynamicsDimension')
+ if dimension == "distance":
+ distance = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
+ else:
+ duration = float(long_maneuver.find("SpeedActionDynamics").attrib.get('value', float("inf")))
+
+ # absolute velocity with given target speed
+ if long_maneuver.find("SpeedActionTarget").find("AbsoluteTargetSpeed") is not None:
+ target_speed = float(long_maneuver.find("SpeedActionTarget").find(
+ "AbsoluteTargetSpeed").attrib.get('value', 0))
+ atomic = ChangeActorTargetSpeed(
+ actor, target_speed, distance=distance, duration=duration, name=maneuver_name)
+
+ # relative velocity to given actor
+ if long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed") is not None:
+ relative_speed = long_maneuver.find("SpeedActionTarget").find("RelativeTargetSpeed")
+ obj = relative_speed.attrib.get('entityRef')
+ value = float(relative_speed.attrib.get('value', 0))
+ value_type = relative_speed.attrib.get('speedTargetValueType')
+ continuous = relative_speed.attrib.get('continuous')
+
+ for traffic_actor in CarlaDataProvider.get_world().get_actors():
+ if 'role_name' in traffic_actor.attributes and traffic_actor.attributes['role_name'] == obj:
+ obj_actor = traffic_actor
+
+ atomic = ChangeActorTargetSpeed(actor,
+ target_speed=0.0,
+ relative_actor=obj_actor,
+ value=value,
+ value_type=value_type,
+ continuous=continuous,
+ distance=distance,
+ duration=duration,
+ name=maneuver_name)
+
+ elif private_action.find('LongitudinalDistanceAction') is not None:
+ raise NotImplementedError("Longitudinal distance actions are not yet supported")
+ else:
+ raise AttributeError("Unknown longitudinal action")
+ elif private_action.find('LateralAction') is not None:
+ private_action = private_action.find('LateralAction')
+ if private_action.find('LaneChangeAction') is not None:
+ # Note: LaneChangeActions are currently only supported for RelativeTargetLane
+ # with +1 or -1 referring to the action actor
+ lat_maneuver = private_action.find('LaneChangeAction')
+ target_lane_rel = float(lat_maneuver.find("LaneChangeTarget").find(
+ "RelativeTargetLane").attrib.get('value', 0))
+ # duration and distance
+ distance = float('inf')
+ duration = float('inf')
+ dimension = lat_maneuver.find("LaneChangeActionDynamics").attrib.get('dynamicsDimension')
+ if dimension == "distance":
+ distance = float(
+ lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
+ else:
+ duration = float(
+ lat_maneuver.find("LaneChangeActionDynamics").attrib.get('value', float("inf")))
+ atomic = ChangeActorLateralMotion(actor,
+ direction="left" if target_lane_rel < 0 else "right",
+ distance_lane_change=distance,
+ distance_other_lane=1000,
+ name=maneuver_name)
+ else:
+ raise AttributeError("Unknown lateral action")
+ elif private_action.find('VisibilityAction') is not None:
+ raise NotImplementedError("Visibility actions are not yet supported")
+ elif private_action.find('SynchronizeAction') is not None:
+ raise NotImplementedError("Synchronization actions are not yet supported")
+ elif private_action.find('ActivateControllerAction') is not None:
+ private_action = private_action.find('ActivateControllerAction')
+ activate = strtobool(private_action.attrib.get('longitudinal'))
+ atomic = ChangeAutoPilot(actor, activate, name=maneuver_name)
+ elif private_action.find('ControllerAction') is not None:
+ controller_action = private_action.find('ControllerAction')
+ module, args = OpenScenarioParser.get_controller(controller_action, catalogs)
+ atomic = ChangeActorControl(actor, control_py_module=module, args=args)
+ elif private_action.find('TeleportAction') is not None:
+ position = private_action.find('TeleportAction')
+ atomic = ActorTransformSetterToOSCPosition(actor, position, name=maneuver_name)
+ elif private_action.find('RoutingAction') is not None:
+ private_action = private_action.find('RoutingAction')
+ if private_action.find('AssignRouteAction') is not None:
+ # @TODO: How to handle relative positions here? This might chance at runtime?!
+ route_action = private_action.find('AssignRouteAction')
+ waypoints = OpenScenarioParser.get_route(route_action, catalogs)
+ atomic = ChangeActorWaypoints(actor, waypoints=waypoints, name=maneuver_name)
+ elif private_action.find('FollowTrajectoryAction') is not None:
+ raise NotImplementedError("Private FollowTrajectory actions are not yet supported")
+ elif private_action.find('AcquirePositionAction') is not None:
+ route_action = private_action.find('AcquirePositionAction')
+ osc_position = route_action.find('Position')
+ position = OpenScenarioParser.convert_position_to_transform(osc_position)
+ atomic = ChangeActorWaypointsToReachPosition(actor, position=position, name=maneuver_name)
+ else:
+ raise AttributeError("Unknown private routing action")
+ else:
+ raise AttributeError("Unknown private action")
+
+ else:
+ if list(action):
+ raise AttributeError("Unknown action: {}".format(maneuver_name))
+ else:
+ return Idle(duration=0, name=maneuver_name)
+
+ return atomic
diff --git a/scenario_runner/srunner/tools/py_trees_port.py b/scenario_runner/srunner/tools/py_trees_port.py
new file mode 100644
index 0000000..cc06bc0
--- /dev/null
+++ b/scenario_runner/srunner/tools/py_trees_port.py
@@ -0,0 +1,137 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2015 Daniel Stonier
+# Copyright (c) 2020 Intel Corporation
+#
+# License: BSD
+# https://raw.githubusercontent.com/splintered-reality/py_trees/devel/LICENSE
+
+"""
+This module provides a backport from newer py_trees releases (> 1.0)
+To use certain features also within ScenarioRunner, which uses py_trees
+version 0.8.x
+"""
+
+import py_trees
+
+
+class Decorator(py_trees.behaviour.Behaviour):
+
+ """
+ A decorator is responsible for handling the lifecycle of a single
+ child beneath
+
+ This is taken from py_trees 1.2 to work with our current implementation
+ that uses py_trees 0.8.2
+ """
+
+ def __init__(self, child, name):
+ """
+ Common initialisation steps for a decorator - type checks and
+ name construction (if None is given).
+ Args:
+ name (:obj:`str`): the decorator name
+ child (:class:`~py_trees.behaviour.Behaviour`): the child to be decorated
+ Raises:
+ TypeError: if the child is not an instance of :class:`~py_trees.behaviour.Behaviour`
+ """
+ # Checks
+ if not isinstance(child, py_trees.behaviour.Behaviour):
+ raise TypeError("A decorator's child must be an instance of py_trees.behaviours.Behaviour")
+ # Initialise
+ super(Decorator, self).__init__(name=name)
+ self.children.append(child)
+ # Give a convenient alias
+ self.decorated = self.children[0]
+ self.decorated.parent = self
+
+ def tick(self):
+ """
+ A decorator's tick is exactly the same as a normal proceedings for
+ a Behaviour's tick except that it also ticks the decorated child node.
+ Yields:
+ :class:`~py_trees.behaviour.Behaviour`: a reference to itself or one of its children
+ """
+ self.logger.debug("%s.tick()" % self.__class__.__name__)
+ # initialise just like other behaviours/composites
+ if self.status != py_trees.common.Status.RUNNING:
+ self.initialise()
+ # interrupt proceedings and process the child node
+ # (including any children it may have as well)
+ for node in self.decorated.tick():
+ yield node
+ # resume normal proceedings for a Behaviour's tick
+ new_status = self.update()
+ if new_status not in list(py_trees.common.Status):
+ self.logger.error(
+ "A behaviour returned an invalid status, setting to INVALID [%s][%s]" % (new_status, self.name))
+ new_status = py_trees.common.Status.INVALID
+ if new_status != py_trees.common.Status.RUNNING:
+ self.stop(new_status)
+ self.status = new_status
+ yield self
+
+ def stop(self, new_status=py_trees.common.Status.INVALID):
+ """
+ As with other composites, it checks if the child is running
+ and stops it if that is the case.
+ Args:
+ new_status (:class:`~py_trees.common.Status`): the behaviour is transitioning to this new status
+ """
+ self.logger.debug("%s.stop(%s)" % (self.__class__.__name__, new_status))
+ self.terminate(new_status)
+ # priority interrupt handling
+ if new_status == py_trees.common.Status.INVALID:
+ self.decorated.stop(new_status)
+ # if the decorator returns SUCCESS/FAILURE and should stop the child
+ if self.decorated.status == py_trees.common.Status.RUNNING:
+ self.decorated.stop(py_trees.common.Status.INVALID)
+ self.status = new_status
+
+ def tip(self):
+ """
+ Get the *tip* of this behaviour's subtree (if it has one) after it's last
+ tick. This corresponds to the the deepest node that was running before the
+ subtree traversal reversed direction and headed back to this node.
+ """
+ if self.decorated.status != py_trees.common.Status.INVALID:
+ return self.decorated.tip()
+
+ return super(Decorator, self).tip()
+
+
+def oneshot_behavior(variable_name, behaviour, name=None):
+ """
+ This is taken from py_trees.idiom.oneshot.
+ """
+ if not name:
+ name = behaviour.name
+
+ subtree_root = py_trees.composites.Selector(name=name)
+
+ # Initialize the variables
+ blackboard = py_trees.blackboard.Blackboard()
+ _ = blackboard.set(variable_name, False)
+
+ # Wait until the scenario has ended
+ check_flag = py_trees.blackboard.CheckBlackboardVariable(
+ name=variable_name + " Done?",
+ variable_name=variable_name,
+ expected_value=True,
+ clearing_policy=py_trees.common.ClearingPolicy.ON_INITIALISE
+ )
+ set_flag = py_trees.blackboard.SetBlackboardVariable(
+ name="Mark Done",
+ variable_name=variable_name,
+ variable_value=True
+ )
+ # If it's a sequence, don't double-nest it in a redundant manner
+ if isinstance(behaviour, py_trees.composites.Sequence):
+ behaviour.add_child(set_flag)
+ sequence = behaviour
+ else:
+ sequence = py_trees.composites.Sequence(name="OneShot")
+ sequence.add_children([behaviour, set_flag])
+
+ subtree_root.add_children([check_flag, sequence])
+ return subtree_root
diff --git a/scenario_runner/srunner/tools/route_manipulation.py b/scenario_runner/srunner/tools/route_manipulation.py
new file mode 100644
index 0000000..0fed088
--- /dev/null
+++ b/scenario_runner/srunner/tools/route_manipulation.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+# Copyright (c) 2018-2019 Intel Labs.
+# authors: German Ros (german.ros@intel.com), Felipe Codevilla (felipe.alcm@gmail.com)
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Module to manipulate the routes, by making then more or less dense (Up to a certain parameter).
+It also contains functions to convert the CARLA world location do GPS coordinates.
+"""
+
+import math
+import xml.etree.ElementTree as ET
+
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+
+from agents.navigation.local_planner import RoadOption
+
+
+def _location_to_gps(lat_ref, lon_ref, location):
+ """
+ Convert from world coordinates to GPS coordinates
+ :param lat_ref: latitude reference for the current map
+ :param lon_ref: longitude reference for the current map
+ :param location: location to translate
+ :return: dictionary with lat, lon and height
+ """
+
+ EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name
+ scale = math.cos(lat_ref * math.pi / 180.0)
+ mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0
+ my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * math.pi / 360.0))
+ mx += location.x
+ my -= location.y
+
+ lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale)
+ lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0
+ z = location.z
+
+ return {'lat': lat, 'lon': lon, 'z': z}
+
+
+def location_route_to_gps(route, lat_ref, lon_ref):
+ """
+ Locate each waypoint of the route into gps, (lat long ) representations.
+ :param route:
+ :param lat_ref:
+ :param lon_ref:
+ :return:
+ """
+ gps_route = []
+
+ for transform, connection in route:
+ gps_point = _location_to_gps(lat_ref, lon_ref, transform.location)
+ gps_route.append((gps_point, connection))
+
+ return gps_route
+
+
+def _get_latlon_ref(world):
+ """
+ Convert from waypoints world coordinates to CARLA GPS coordinates
+ :return: tuple with lat and lon coordinates
+ """
+ xodr = world.get_map().to_opendrive()
+ tree = ET.ElementTree(ET.fromstring(xodr))
+
+ # default reference
+ lat_ref = 42.0
+ lon_ref = 2.0
+
+ for opendrive in tree.iter("OpenDRIVE"):
+ for header in opendrive.iter("header"):
+ for georef in header.iter("geoReference"):
+ if georef.text:
+ str_list = georef.text.split(' ')
+ for item in str_list:
+ if '+lat_0' in item:
+ lat_ref = float(item.split('=')[1])
+ if '+lon_0' in item:
+ lon_ref = float(item.split('=')[1])
+ return lat_ref, lon_ref
+
+
+def downsample_route(route, sample_factor):
+ """
+ Downsample the route by some factor.
+ :param route: the trajectory , has to contain the waypoints and the road options
+ :param sample_factor: Maximum distance between samples
+ :return: returns the ids of the final route that can
+ """
+
+ ids_to_sample = []
+ prev_option = None
+ dist = 0
+
+ for i, point in enumerate(route):
+ curr_option = point[1]
+
+ # Lane changing
+ if curr_option in (RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT):
+ ids_to_sample.append(i)
+ dist = 0
+
+ # When road option changes
+ elif prev_option != curr_option and prev_option not in (RoadOption.CHANGELANELEFT, RoadOption.CHANGELANERIGHT):
+ ids_to_sample.append(i)
+ dist = 0
+
+ # After a certain max distance
+ elif dist > sample_factor:
+ ids_to_sample.append(i)
+ dist = 0
+
+ # At the end
+ elif i == len(route) - 1:
+ ids_to_sample.append(i)
+ dist = 0
+
+ # Compute the distance traveled
+ else:
+ curr_location = point[0].location
+ prev_location = route[i - 1][0].location
+ dist += curr_location.distance(prev_location)
+
+ prev_option = curr_option
+
+ return ids_to_sample
+
+
+def interpolate_trajectory(world, waypoints_trajectory, hop_resolution=1.0):
+ """
+ Given some raw keypoints interpolate a full dense trajectory to be used by the user.
+ :param world: an reference to the CARLA world so we can use the planner
+ :param waypoints_trajectory: the current coarse trajectory
+ :param hop_resolution: is the resolution, how dense is the provided trajectory going to be made
+ :return: the full interpolated route both in GPS coordinates and also in its original form.
+ """
+
+ dao = GlobalRoutePlannerDAO(world.get_map(), hop_resolution)
+ grp = GlobalRoutePlanner(dao)
+ grp.setup()
+ # Obtain route plan
+ route = []
+ for i in range(len(waypoints_trajectory) - 1): # Goes until the one before the last.
+
+ waypoint = waypoints_trajectory[i]
+ waypoint_next = waypoints_trajectory[i + 1]
+ interpolated_trace = grp.trace_route(waypoint, waypoint_next)
+ for wp_tuple in interpolated_trace:
+ route.append((wp_tuple[0].transform, wp_tuple[1]))
+
+ # Increase the route position to avoid fails
+
+ lat_ref, lon_ref = _get_latlon_ref(world)
+
+ return location_route_to_gps(route, lat_ref, lon_ref), route
diff --git a/scenario_runner/srunner/tools/route_parser.py b/scenario_runner/srunner/tools/route_parser.py
new file mode 100644
index 0000000..bff929c
--- /dev/null
+++ b/scenario_runner/srunner/tools/route_parser.py
@@ -0,0 +1,325 @@
+#!/usr/bin/env python
+
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Module used to parse all the route and scenario configuration parameters.
+"""
+
+import json
+import math
+import xml.etree.ElementTree as ET
+
+import carla
+from agents.navigation.local_planner import RoadOption
+from srunner.scenarioconfigs.route_scenario_configuration import RouteScenarioConfiguration
+
+# TODO check this threshold, it could be a bit larger but not so large that we cluster scenarios.
+TRIGGER_THRESHOLD = 2.0 # Threshold to say if a trigger position is new or repeated, works for matching positions
+TRIGGER_ANGLE_THRESHOLD = 10 # Threshold to say if two angles can be considering matching when matching transforms.
+
+
+class RouteParser(object):
+
+ """
+ Pure static class used to parse all the route and scenario configuration parameters.
+ """
+
+ @staticmethod
+ def parse_annotations_file(annotation_filename):
+ """
+ Return the annotations of which positions where the scenarios are going to happen.
+ :param annotation_filename: the filename for the anotations file
+ :return:
+ """
+
+ with open(annotation_filename, 'r') as f:
+ annotation_dict = json.loads(f.read())
+
+ final_dict = {}
+
+ for town_dict in annotation_dict['available_scenarios']:
+ final_dict.update(town_dict)
+
+ return final_dict # the file has a current maps name that is an one element vec
+
+ @staticmethod
+ def parse_routes_file(route_filename, scenario_file, single_route=None):
+ """
+ Returns a list of route elements.
+ :param route_filename: the path to a set of routes.
+ :param single_route: If set, only this route shall be returned
+ :return: List of dicts containing the waypoints, id and town of the routes
+ """
+
+ list_route_descriptions = []
+ tree = ET.parse(route_filename)
+ for route in tree.iter("route"):
+
+ route_id = route.attrib['id']
+ if single_route and route_id != single_route:
+ continue
+
+ new_config = RouteScenarioConfiguration()
+ new_config.town = route.attrib['town']
+ new_config.name = "RouteScenario_{}".format(route_id)
+ new_config.weather = RouteParser.parse_weather(route)
+ new_config.scenario_file = scenario_file
+
+ waypoint_list = [] # the list of waypoints that can be found on this route
+ for waypoint in route.iter('waypoint'):
+ waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']),
+ y=float(waypoint.attrib['y']),
+ z=float(waypoint.attrib['z'])))
+
+ new_config.trajectory = waypoint_list
+
+ list_route_descriptions.append(new_config)
+
+ return list_route_descriptions
+
+ @staticmethod
+ def parse_weather(route):
+ """
+ Returns a carla.WeatherParameters with the corresponding weather for that route. If the route
+ has no weather attribute, the default one is triggered.
+ """
+
+ route_weather = route.find("weather")
+ if route_weather is None:
+
+ weather = carla.WeatherParameters(sun_altitude_angle=70)
+
+ else:
+ weather = carla.WeatherParameters()
+ for weather_attrib in route.iter("weather"):
+
+ if 'cloudiness' in weather_attrib.attrib:
+ weather.cloudiness = float(weather_attrib.attrib['cloudiness'])
+ if 'precipitation' in weather_attrib.attrib:
+ weather.precipitation = float(weather_attrib.attrib['precipitation'])
+ if 'precipitation_deposits' in weather_attrib.attrib:
+ weather.precipitation_deposits = float(weather_attrib.attrib['precipitation_deposits'])
+ if 'wind_intensity' in weather_attrib.attrib:
+ weather.wind_intensity = float(weather_attrib.attrib['wind_intensity'])
+ if 'sun_azimuth_angle' in weather_attrib.attrib:
+ weather.sun_azimuth_angle = float(weather_attrib.attrib['sun_azimuth_angle'])
+ if 'sun_altitude_angle' in weather_attrib.attrib:
+ weather.sun_altitude_angle = float(weather_attrib.attrib['sun_altitude_angle'])
+ if 'wetness' in weather_attrib.attrib:
+ weather.wetness = float(weather_attrib.attrib['wetness'])
+ if 'fog_distance' in weather_attrib.attrib:
+ weather.fog_distance = float(weather_attrib.attrib['fog_distance'])
+ if 'fog_density' in weather_attrib.attrib:
+ weather.fog_density = float(weather_attrib.attrib['fog_density'])
+
+ return weather
+
+ @staticmethod
+ def check_trigger_position(new_trigger, existing_triggers):
+ """
+ Check if this trigger position already exists or if it is a new one.
+ :param new_trigger:
+ :param existing_triggers:
+ :return:
+ """
+
+ for trigger_id in existing_triggers.keys():
+ trigger = existing_triggers[trigger_id]
+ dx = trigger['x'] - new_trigger['x']
+ dy = trigger['y'] - new_trigger['y']
+ distance = math.sqrt(dx * dx + dy * dy)
+
+ dyaw = (trigger['yaw'] - new_trigger['yaw']) % 360
+ if distance < TRIGGER_THRESHOLD \
+ and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD)):
+ return trigger_id
+
+ return None
+
+ @staticmethod
+ def convert_waypoint_float(waypoint):
+ """
+ Convert waypoint values to float
+ """
+ waypoint['x'] = float(waypoint['x'])
+ waypoint['y'] = float(waypoint['y'])
+ waypoint['z'] = float(waypoint['z'])
+ waypoint['yaw'] = float(waypoint['yaw'])
+
+ @staticmethod
+ def match_world_location_to_route(world_location, route_description):
+ """
+ We match this location to a given route.
+ world_location:
+ route_description:
+ """
+ def match_waypoints(waypoint1, wtransform):
+ """
+ Check if waypoint1 and wtransform are similar
+ """
+ dx = float(waypoint1['x']) - wtransform.location.x
+ dy = float(waypoint1['y']) - wtransform.location.y
+ dz = float(waypoint1['z']) - wtransform.location.z
+ dpos = math.sqrt(dx * dx + dy * dy + dz * dz)
+
+ dyaw = (float(waypoint1['yaw']) - wtransform.rotation.yaw) % 360
+
+ return dpos < TRIGGER_THRESHOLD \
+ and (dyaw < TRIGGER_ANGLE_THRESHOLD or dyaw > (360 - TRIGGER_ANGLE_THRESHOLD))
+
+ match_position = 0
+ # TODO this function can be optimized to run on Log(N) time
+ for route_waypoint in route_description:
+ if match_waypoints(world_location, route_waypoint[0]):
+ return match_position
+ match_position += 1
+
+ return None
+
+ @staticmethod
+ def get_scenario_type(scenario, match_position, trajectory):
+ """
+ Some scenarios have different types depending on the route.
+ :param scenario: the scenario name
+ :param match_position: the matching position for the scenarion
+ :param trajectory: the route trajectory the ego is following
+ :return: tag representing this subtype
+
+ Also used to check which are not viable (Such as an scenario
+ that triggers when turning but the route doesnt')
+ WARNING: These tags are used at:
+ - VehicleTurningRoute
+ - SignalJunctionCrossingRoute
+ and changes to these tags will affect them
+ """
+
+ def check_this_waypoint(tuple_wp_turn):
+ """
+ Decides whether or not the waypoint will define the scenario behavior
+ """
+ if RoadOption.LANEFOLLOW == tuple_wp_turn[1]:
+ return False
+ elif RoadOption.CHANGELANELEFT == tuple_wp_turn[1]:
+ return False
+ elif RoadOption.CHANGELANERIGHT == tuple_wp_turn[1]:
+ return False
+ return True
+
+ # Unused tag for the rest of scenarios,
+ # can't be None as they are still valid scenarios
+ subtype = 'valid'
+
+ if scenario == 'Scenario4':
+ for tuple_wp_turn in trajectory[match_position:]:
+ if check_this_waypoint(tuple_wp_turn):
+ if RoadOption.LEFT == tuple_wp_turn[1]:
+ subtype = 'S4left'
+ elif RoadOption.RIGHT == tuple_wp_turn[1]:
+ subtype = 'S4right'
+ else:
+ subtype = None
+ break # Avoid checking all of them
+ subtype = None
+
+ if scenario == 'Scenario7':
+ for tuple_wp_turn in trajectory[match_position:]:
+ if check_this_waypoint(tuple_wp_turn):
+ if RoadOption.LEFT == tuple_wp_turn[1]:
+ subtype = 'S7left'
+ elif RoadOption.RIGHT == tuple_wp_turn[1]:
+ subtype = 'S7right'
+ elif RoadOption.STRAIGHT == tuple_wp_turn[1]:
+ subtype = 'S7opposite'
+ else:
+ subtype = None
+ break # Avoid checking all of them
+ subtype = None
+
+ if scenario == 'Scenario8':
+ for tuple_wp_turn in trajectory[match_position:]:
+ if check_this_waypoint(tuple_wp_turn):
+ if RoadOption.LEFT == tuple_wp_turn[1]:
+ subtype = 'S8left'
+ else:
+ subtype = None
+ break # Avoid checking all of them
+ subtype = None
+
+ if scenario == 'Scenario9':
+ for tuple_wp_turn in trajectory[match_position:]:
+ if check_this_waypoint(tuple_wp_turn):
+ if RoadOption.RIGHT == tuple_wp_turn[1]:
+ subtype = 'S9right'
+ else:
+ subtype = None
+ break # Avoid checking all of them
+ subtype = None
+
+ return subtype
+
+ @staticmethod
+ def scan_route_for_scenarios(route_name, trajectory, world_annotations):
+ """
+ Just returns a plain list of possible scenarios that can happen in this route by matching
+ the locations from the scenario into the route description
+
+ :return: A list of scenario definitions with their correspondent parameters
+ """
+
+ # the triggers dictionaries:
+ existent_triggers = {}
+ # We have a table of IDs and trigger positions associated
+ possible_scenarios = {}
+
+ # Keep track of the trigger ids being added
+ latest_trigger_id = 0
+
+ for town_name in world_annotations.keys():
+ if town_name != route_name:
+ continue
+
+ scenarios = world_annotations[town_name]
+ for scenario in scenarios: # For each existent scenario
+ if "scenario_type" not in scenario:
+ break
+ scenario_name = scenario["scenario_type"]
+ for event in scenario["available_event_configurations"]:
+ waypoint = event['transform'] # trigger point of this scenario
+ RouteParser.convert_waypoint_float(waypoint)
+ # We match trigger point to the route, now we need to check if the route affects
+ match_position = RouteParser.match_world_location_to_route(
+ waypoint, trajectory)
+ if match_position is not None:
+ # We match a location for this scenario, create a scenario object so this scenario
+ # can be instantiated later
+
+ if 'other_actors' in event:
+ other_vehicles = event['other_actors']
+ else:
+ other_vehicles = None
+ scenario_subtype = RouteParser.get_scenario_type(scenario_name, match_position,
+ trajectory)
+ if scenario_subtype is None:
+ continue
+ scenario_description = {
+ 'name': scenario_name,
+ 'other_actors': other_vehicles,
+ 'trigger_position': waypoint,
+ 'scenario_type': scenario_subtype, # some scenarios have route dependent configs
+ }
+
+ trigger_id = RouteParser.check_trigger_position(waypoint, existent_triggers)
+ if trigger_id is None:
+ # This trigger does not exist create a new reference on existent triggers
+ existent_triggers.update({latest_trigger_id: waypoint})
+ # Update a reference for this trigger on the possible scenarios
+ possible_scenarios.update({latest_trigger_id: []})
+ trigger_id = latest_trigger_id
+ # Increment the latest trigger
+ latest_trigger_id += 1
+
+ possible_scenarios[trigger_id].append(scenario_description)
+
+ return possible_scenarios, existent_triggers
diff --git a/scenario_runner/srunner/tools/scenario_helper.py b/scenario_runner/srunner/tools/scenario_helper.py
new file mode 100644
index 0000000..ee14b82
--- /dev/null
+++ b/scenario_runner/srunner/tools/scenario_helper.py
@@ -0,0 +1,510 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+Summary of useful helper functions for scenarios
+"""
+
+import math
+import shapely.geometry
+import shapely.affinity
+
+import numpy as np
+
+import carla
+from agents.tools.misc import vector
+from agents.navigation.local_planner import RoadOption
+
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+
+
+def get_distance_along_route(route, target_location):
+ """
+ Calculate the distance of the given location along the route
+
+ Note: If the location is not along the route, the route length will be returned
+ """
+
+ wmap = CarlaDataProvider.get_map()
+ covered_distance = 0
+ prev_position = None
+ found = False
+
+ # Don't use the input location, use the corresponding wp as location
+ target_location_from_wp = wmap.get_waypoint(target_location).transform.location
+
+ for position, _ in route:
+
+ location = target_location_from_wp
+
+ # Don't perform any calculations for the first route point
+ if not prev_position:
+ prev_position = position
+ continue
+
+ # Calculate distance between previous and current route point
+ interval_length_squared = ((prev_position.x - position.x) ** 2) + ((prev_position.y - position.y) ** 2)
+ distance_squared = ((location.x - prev_position.x) ** 2) + ((location.y - prev_position.y) ** 2)
+
+ # Close to the current position? Stop calculation
+ if distance_squared < 0.01:
+ break
+
+ if distance_squared < 400 and not distance_squared < interval_length_squared:
+ # Check if a neighbor lane is closer to the route
+ # Do this only in a close distance to correct route interval, otherwise the computation load is too high
+ starting_wp = wmap.get_waypoint(location)
+ wp = starting_wp.get_left_lane()
+ while wp is not None:
+ new_location = wp.transform.location
+ new_distance_squared = ((new_location.x - prev_position.x) ** 2) + (
+ (new_location.y - prev_position.y) ** 2)
+
+ if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
+ break
+
+ if new_distance_squared < distance_squared:
+ distance_squared = new_distance_squared
+ location = new_location
+ else:
+ break
+
+ wp = wp.get_left_lane()
+
+ wp = starting_wp.get_right_lane()
+ while wp is not None:
+ new_location = wp.transform.location
+ new_distance_squared = ((new_location.x - prev_position.x) ** 2) + (
+ (new_location.y - prev_position.y) ** 2)
+
+ if np.sign(starting_wp.lane_id) != np.sign(wp.lane_id):
+ break
+
+ if new_distance_squared < distance_squared:
+ distance_squared = new_distance_squared
+ location = new_location
+ else:
+ break
+
+ wp = wp.get_right_lane()
+
+ if distance_squared < interval_length_squared:
+ # The location could be inside the current route interval, if route/lane ids match
+ # Note: This assumes a sufficiently small route interval
+ # An alternative is to compare orientations, however, this also does not work for
+ # long route intervals
+
+ curr_wp = wmap.get_waypoint(position)
+ prev_wp = wmap.get_waypoint(prev_position)
+ wp = wmap.get_waypoint(location)
+
+ if prev_wp and curr_wp and wp:
+ if wp.road_id == prev_wp.road_id or wp.road_id == curr_wp.road_id:
+ # Roads match, now compare the sign of the lane ids
+ if (np.sign(wp.lane_id) == np.sign(prev_wp.lane_id) or
+ np.sign(wp.lane_id) == np.sign(curr_wp.lane_id)):
+ # The location is within the current route interval
+ covered_distance += math.sqrt(distance_squared)
+ found = True
+ break
+
+ covered_distance += math.sqrt(interval_length_squared)
+ prev_position = position
+
+ return covered_distance, found
+
+
+def get_crossing_point(actor):
+ """
+ Get the next crossing point location in front of the ego vehicle
+
+ @return point of crossing
+ """
+ wp_cross = CarlaDataProvider.get_map().get_waypoint(actor.get_location())
+
+ while not wp_cross.is_intersection:
+ wp_cross = wp_cross.next(2)[0]
+
+ crossing = carla.Location(x=wp_cross.transform.location.x,
+ y=wp_cross.transform.location.y, z=wp_cross.transform.location.z)
+
+ return crossing
+
+
+def get_geometric_linear_intersection(ego_actor, other_actor):
+ """
+ Obtain a intersection point between two actor's location by using their waypoints (wp)
+
+ @return point of intersection of the two vehicles
+ """
+
+ wp_ego_1 = CarlaDataProvider.get_map().get_waypoint(ego_actor.get_location())
+ wp_ego_2 = wp_ego_1.next(1)[0]
+ x_ego_1 = wp_ego_1.transform.location.x
+ y_ego_1 = wp_ego_1.transform.location.y
+ x_ego_2 = wp_ego_2.transform.location.x
+ y_ego_2 = wp_ego_2.transform.location.y
+
+ wp_other_1 = CarlaDataProvider.get_world().get_map().get_waypoint(other_actor.get_location())
+ wp_other_2 = wp_other_1.next(1)[0]
+ x_other_1 = wp_other_1.transform.location.x
+ y_other_1 = wp_other_1.transform.location.y
+ x_other_2 = wp_other_2.transform.location.x
+ y_other_2 = wp_other_2.transform.location.y
+
+ s = np.vstack([(x_ego_1, y_ego_1), (x_ego_2, y_ego_2), (x_other_1, y_other_1), (x_other_2, y_other_2)])
+ h = np.hstack((s, np.ones((4, 1))))
+ line1 = np.cross(h[0], h[1])
+ line2 = np.cross(h[2], h[3])
+ x, y, z = np.cross(line1, line2)
+ if z == 0:
+ return (float('inf'), float('inf'))
+
+ intersection = carla.Location(x=x / z, y=y / z, z=0)
+
+ return intersection
+
+
+def get_location_in_distance(actor, distance):
+ """
+ Obtain a location in a given distance from the current actor's location.
+ Note: Search is stopped on first intersection.
+
+ @return obtained location and the traveled distance
+ """
+ waypoint = CarlaDataProvider.get_map().get_waypoint(actor.get_location())
+ traveled_distance = 0
+ while not waypoint.is_intersection and traveled_distance < distance:
+ waypoint_new = waypoint.next(1.0)[-1]
+ traveled_distance += waypoint_new.transform.location.distance(waypoint.transform.location)
+ waypoint = waypoint_new
+
+ return waypoint.transform.location, traveled_distance
+
+
+def get_location_in_distance_from_wp(waypoint, distance, stop_at_junction=True):
+ """
+ Obtain a location in a given distance from the current actor's location.
+ Note: Search is stopped on first intersection.
+
+ @return obtained location and the traveled distance
+ """
+ traveled_distance = 0
+ while not (waypoint.is_intersection and stop_at_junction) and traveled_distance < distance:
+ wp_next = waypoint.next(1.0)
+ if wp_next:
+ waypoint_new = wp_next[-1]
+ traveled_distance += waypoint_new.transform.location.distance(waypoint.transform.location)
+ waypoint = waypoint_new
+ else:
+ break
+
+ return waypoint.transform.location, traveled_distance
+
+
+def get_waypoint_in_distance(waypoint, distance):
+ """
+ Obtain a waypoint in a given distance from the current actor's location.
+ Note: Search is stopped on first intersection.
+ @return obtained waypoint and the traveled distance
+ """
+ traveled_distance = 0
+ while not waypoint.is_intersection and traveled_distance < distance:
+ waypoint_new = waypoint.next(1.0)[-1]
+ traveled_distance += waypoint_new.transform.location.distance(waypoint.transform.location)
+ waypoint = waypoint_new
+
+ return waypoint, traveled_distance
+
+
+def generate_target_waypoint_list(waypoint, turn=0):
+ """
+ This method follow waypoints to a junction and choose path based on turn input.
+ Turn input: LEFT -> -1, RIGHT -> 1, STRAIGHT -> 0
+ @returns a waypoint list from the starting point to the end point according to turn input
+ """
+ reached_junction = False
+ threshold = math.radians(0.1)
+ plan = []
+ while True:
+ wp_choice = waypoint.next(2)
+ if len(wp_choice) > 1:
+ reached_junction = True
+ waypoint = choose_at_junction(waypoint, wp_choice, turn)
+ else:
+ waypoint = wp_choice[0]
+ plan.append((waypoint, RoadOption.LANEFOLLOW))
+ # End condition for the behavior
+ if turn != 0 and reached_junction and len(plan) >= 3:
+ v_1 = vector(
+ plan[-2][0].transform.location,
+ plan[-1][0].transform.location)
+ v_2 = vector(
+ plan[-3][0].transform.location,
+ plan[-2][0].transform.location)
+ angle_wp = math.acos(
+ np.dot(v_1, v_2) / abs((np.linalg.norm(v_1) * np.linalg.norm(v_2))))
+ if angle_wp < threshold:
+ break
+ elif reached_junction and not plan[-1][0].is_intersection:
+ break
+
+ return plan, plan[-1][0]
+
+
+def generate_target_waypoint_list_multilane(waypoint, change='left',
+ distance_same_lane=10,
+ distance_other_lane=25,
+ total_lane_change_distance=25, check='true'):
+ """
+ This methods generates a waypoint list which leads the vehicle to a parallel lane.
+ The change input must be 'left' or 'right', depending on which lane you want to change.
+
+ The step distance between waypoints on the same lane is 2m.
+ The step distance between the lane change is set to 25m.
+
+ @returns a waypoint list from the starting point to the end point on a right or left parallel lane.
+ """
+ plan = []
+ plan.append((waypoint, RoadOption.LANEFOLLOW)) # start position
+
+ step_distance = 2
+
+ # check if lane change possible
+ if check == 'true':
+ lane_change_possibilities = ['Left', 'Right', 'Both']
+ if str(waypoint.lane_change) not in lane_change_possibilities:
+ # ERROR, lane change is not possible
+ return None
+
+ # same lane
+ distance = 0
+ while distance < distance_same_lane:
+ next_wp = plan[-1][0].next(step_distance)
+ distance += next_wp[0].transform.location.distance(plan[-1][0].transform.location)
+ plan.append((next_wp[0], RoadOption.LANEFOLLOW))
+
+ target_lane_id = None
+ if change == 'left':
+ # go left
+ wp_left = plan[-1][0].get_left_lane()
+ target_lane_id = wp_left.lane_id
+ next_wp = wp_left.next(total_lane_change_distance)
+ plan.append((next_wp[0], RoadOption.LANEFOLLOW))
+ elif change == 'right':
+ # go right
+ wp_right = plan[-1][0].get_right_lane()
+ target_lane_id = wp_right.lane_id
+ next_wp = wp_right.next(total_lane_change_distance)
+ plan.append((next_wp[0], RoadOption.LANEFOLLOW))
+ else:
+ # ERROR, input value for change must be 'left' or 'right'
+ return None
+
+ # other lane
+ distance = 0
+ while distance < distance_other_lane:
+ next_wp = plan[-1][0].next(step_distance)
+ distance += next_wp[0].transform.location.distance(plan[-1][0].transform.location)
+ plan.append((next_wp[0], RoadOption.LANEFOLLOW))
+
+ return plan, target_lane_id
+
+
+def generate_target_waypoint(waypoint, turn=0):
+ """
+ This method follow waypoints to a junction and choose path based on turn input.
+ Turn input: LEFT -> -1, RIGHT -> 1, STRAIGHT -> 0
+ @returns a waypoint list according to turn input
+ """
+ sampling_radius = 1
+ reached_junction = False
+ wp_list = []
+ while True:
+
+ wp_choice = waypoint.next(sampling_radius)
+ # Choose path at intersection
+ if not reached_junction and (len(wp_choice) > 1 or wp_choice[0].is_junction):
+ reached_junction = True
+ waypoint = choose_at_junction(waypoint, wp_choice, turn)
+ else:
+ waypoint = wp_choice[0]
+ wp_list.append(waypoint)
+ # End condition for the behavior
+ if reached_junction and not wp_list[-1].is_junction:
+ break
+ return wp_list[-1]
+
+
+def generate_target_waypoint_in_route(waypoint, route):
+ """
+ This method follow waypoints to a junction
+ @returns a waypoint list according to turn input
+ """
+ wmap = CarlaDataProvider.get_map()
+ reached_junction = False
+
+ # Get the route location
+ shortest_distance = float('inf')
+ for index, route_pos in enumerate(route):
+ wp = route_pos[0]
+ trigger_location = waypoint.transform.location
+
+ dist_to_route = trigger_location.distance(wp)
+ if dist_to_route <= shortest_distance:
+ closest_index = index
+ shortest_distance = dist_to_route
+
+ route_location = route[closest_index][0]
+ index = closest_index
+
+ while True:
+ # Get the next route location
+ index = min(index + 1, len(route))
+ route_location = route[index][0]
+ road_option = route[index][1]
+
+ # Enter the junction
+ if not reached_junction and (road_option in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)):
+ reached_junction = True
+
+ # End condition for the behavior, at the end of the junction
+ if reached_junction and (road_option not in (RoadOption.LEFT, RoadOption.RIGHT, RoadOption.STRAIGHT)):
+ break
+
+ return wmap.get_waypoint(route_location)
+
+
+def choose_at_junction(current_waypoint, next_choices, direction=0):
+ """
+ This function chooses the appropriate waypoint from next_choices based on direction
+ """
+ current_transform = current_waypoint.transform
+ current_location = current_transform.location
+ projected_location = current_location + \
+ carla.Location(
+ x=math.cos(math.radians(current_transform.rotation.yaw)),
+ y=math.sin(math.radians(current_transform.rotation.yaw)))
+ current_vector = vector(current_location, projected_location)
+ cross_list = []
+ cross_to_waypoint = dict()
+ for waypoint in next_choices:
+ waypoint = waypoint.next(10)[0]
+ select_vector = vector(current_location, waypoint.transform.location)
+ cross = np.cross(current_vector, select_vector)[2]
+ cross_list.append(cross)
+ cross_to_waypoint[cross] = waypoint
+ select_cross = None
+ if direction > 0:
+ select_cross = max(cross_list)
+ elif direction < 0:
+ select_cross = min(cross_list)
+ else:
+ select_cross = min(cross_list, key=abs)
+
+ return cross_to_waypoint[select_cross]
+
+
+def get_intersection(ego_actor, other_actor):
+ """
+ Obtain a intersection point between two actor's location
+ @return the intersection location
+ """
+ waypoint = CarlaDataProvider.get_map().get_waypoint(ego_actor.get_location())
+ waypoint_other = CarlaDataProvider.get_map().get_waypoint(other_actor.get_location())
+ max_dist = float("inf")
+ distance = float("inf")
+ while distance <= max_dist:
+ max_dist = distance
+ current_location = waypoint.transform.location
+ waypoint_choice = waypoint.next(1)
+ # Select the straighter path at intersection
+ if len(waypoint_choice) > 1:
+ max_dot = -1 * float('inf')
+ loc_projection = current_location + carla.Location(
+ x=math.cos(math.radians(waypoint.transform.rotation.yaw)),
+ y=math.sin(math.radians(waypoint.transform.rotation.yaw)))
+ v_current = vector(current_location, loc_projection)
+ for wp_select in waypoint_choice:
+ v_select = vector(current_location, wp_select.transform.location)
+ dot_select = np.dot(v_current, v_select)
+ if dot_select > max_dot:
+ max_dot = dot_select
+ waypoint = wp_select
+ else:
+ waypoint = waypoint_choice[0]
+ distance = current_location.distance(waypoint_other.transform.location)
+
+ return current_location
+
+
+def detect_lane_obstacle(actor, extension_factor=3, margin=1.02):
+ """
+ This function identifies if an obstacle is present in front of the reference actor
+ """
+ world = CarlaDataProvider.get_world()
+ world_actors = world.get_actors().filter('vehicle.*')
+ actor_bbox = actor.bounding_box
+ actor_transform = actor.get_transform()
+ actor_location = actor_transform.location
+ actor_vector = actor_transform.rotation.get_forward_vector()
+ actor_vector = np.array([actor_vector.x, actor_vector.y])
+ actor_vector = actor_vector / np.linalg.norm(actor_vector)
+ actor_vector = actor_vector * (extension_factor - 1) * actor_bbox.extent.x
+ actor_location = actor_location + carla.Location(actor_vector[0], actor_vector[1])
+ actor_yaw = actor_transform.rotation.yaw
+
+ is_hazard = False
+ for adversary in world_actors:
+ if adversary.id != actor.id and \
+ actor_transform.location.distance(adversary.get_location()) < 50:
+ adversary_bbox = adversary.bounding_box
+ adversary_transform = adversary.get_transform()
+ adversary_loc = adversary_transform.location
+ adversary_yaw = adversary_transform.rotation.yaw
+ overlap_adversary = RotatedRectangle(
+ adversary_loc.x, adversary_loc.y,
+ 2 * margin * adversary_bbox.extent.x, 2 * margin * adversary_bbox.extent.y, adversary_yaw)
+ overlap_actor = RotatedRectangle(
+ actor_location.x, actor_location.y,
+ 2 * margin * actor_bbox.extent.x * extension_factor, 2 * margin * actor_bbox.extent.y, actor_yaw)
+ overlap_area = overlap_adversary.intersection(overlap_actor).area
+ if overlap_area > 0:
+ is_hazard = True
+ break
+
+ return is_hazard
+
+
+class RotatedRectangle(object):
+
+ """
+ This class contains method to draw rectangle and find intersection point.
+ """
+
+ def __init__(self, c_x, c_y, width, height, angle):
+ self.c_x = c_x
+ self.c_y = c_y
+ self.w = width # pylint: disable=invalid-name
+ self.h = height # pylint: disable=invalid-name
+ self.angle = angle
+
+ def get_contour(self):
+ """
+ create contour
+ """
+ w = self.w
+ h = self.h
+ c = shapely.geometry.box(-w / 2.0, -h / 2.0, w / 2.0, h / 2.0)
+ rc = shapely.affinity.rotate(c, self.angle)
+ return shapely.affinity.translate(rc, self.c_x, self.c_y)
+
+ def intersection(self, other):
+ """
+ Obtain a intersection point between two contour.
+ """
+ return self.get_contour().intersection(other.get_contour())
diff --git a/scenario_runner/srunner/tools/scenario_parser.py b/scenario_runner/srunner/tools/scenario_parser.py
new file mode 100644
index 0000000..39f9138
--- /dev/null
+++ b/scenario_runner/srunner/tools/scenario_parser.py
@@ -0,0 +1,124 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see .
+
+"""
+This module provides access to a scenario configuration parser
+"""
+
+import glob
+import os
+import xml.etree.ElementTree as ET
+
+from srunner.scenarioconfigs.scenario_configuration import ScenarioConfiguration, ActorConfigurationData
+from srunner.scenarioconfigs.route_scenario_configuration import RouteConfiguration
+
+
+class ScenarioConfigurationParser(object):
+
+ """
+ Pure static class providing access to parser methods for scenario configuration files (*.xml)
+ """
+
+ @staticmethod
+ def parse_scenario_configuration(scenario_name, config_file_name):
+ """
+ Parse all scenario configuration files at srunner/examples and the additional
+ config files, providing a list of ScenarioConfigurations @return
+
+ If scenario_name starts with "group:" all scenarios that
+ have that type are parsed and returned. Otherwise only the
+ scenario that matches the scenario_name is parsed and returned.
+ """
+
+ list_of_config_files = glob.glob("{}/srunner/examples/*.xml".format(os.getenv('SCENARIO_RUNNER_ROOT', "./")))
+
+ if config_file_name != '':
+ list_of_config_files.append(config_file_name)
+
+ single_scenario_only = True
+ if scenario_name.startswith("group:"):
+ single_scenario_only = False
+ scenario_name = scenario_name[6:]
+
+ scenario_configurations = []
+
+ for file_name in list_of_config_files:
+ tree = ET.parse(file_name)
+
+ for scenario in tree.iter("scenario"):
+
+ scenario_config_name = scenario.attrib.get('name', None)
+ scenario_config_type = scenario.attrib.get('type', None)
+
+ if single_scenario_only:
+ # Check the scenario is the correct one
+ if scenario_config_name != scenario_name:
+ continue
+ else:
+ # Check the scenario is of the correct type
+ if scenario_config_type != scenario_name:
+ continue
+
+ new_config = ScenarioConfiguration()
+ new_config.town = scenario.attrib.get('town', None)
+ new_config.name = scenario_config_name
+ new_config.type = scenario_config_type
+ new_config.other_actors = []
+ new_config.ego_vehicles = []
+ new_config.trigger_points = []
+
+ for weather in scenario.iter("weather"):
+ new_config.weather.cloudiness = float(weather.attrib.get("cloudiness", 0))
+ new_config.weather.precipitation = float(weather.attrib.get("precipitation", 0))
+ new_config.weather.precipitation_deposits = float(weather.attrib.get("precipitation_deposits", 0))
+ new_config.weather.wind_intensity = float(weather.attrib.get("wind_intensity", 0.35))
+ new_config.weather.sun_azimuth_angle = float(weather.attrib.get("sun_azimuth_angle", 0.0))
+ new_config.weather.sun_altitude_angle = float(weather.attrib.get("sun_altitude_angle", 15.0))
+ new_config.weather.fog_density = float(weather.attrib.get("fog_density", 0.0))
+ new_config.weather.fog_distance = float(weather.attrib.get("fog_distance", 0.0))
+ new_config.weather.wetness = float(weather.attrib.get("wetness", 0.0))
+
+ for ego_vehicle in scenario.iter("ego_vehicle"):
+
+ new_config.ego_vehicles.append(ActorConfigurationData.parse_from_node(ego_vehicle, 'hero'))
+ new_config.trigger_points.append(new_config.ego_vehicles[-1].transform)
+
+ for route in scenario.iter("route"):
+ route_conf = RouteConfiguration()
+ route_conf.parse_xml(route)
+ new_config.route = route_conf
+
+ for other_actor in scenario.iter("other_actor"):
+ new_config.other_actors.append(ActorConfigurationData.parse_from_node(other_actor, 'scenario'))
+
+ scenario_configurations.append(new_config)
+
+ return scenario_configurations
+
+ @staticmethod
+ def get_list_of_scenarios(config_file_name):
+ """
+ Parse *all* config files and provide a list with all scenarios @return
+ """
+
+ list_of_config_files = glob.glob("{}/srunner/examples/*.xml".format(os.getenv('SCENARIO_RUNNER_ROOT', "./")))
+ list_of_config_files += glob.glob("{}/srunner/examples/*.xosc".format(os.getenv('SCENARIO_RUNNER_ROOT', "./")))
+
+ if config_file_name != '':
+ list_of_config_files.append(config_file_name)
+
+ scenarios = []
+ for file_name in list_of_config_files:
+ if ".xosc" in file_name:
+ tree = ET.parse(file_name)
+ scenarios.append("{} (OpenSCENARIO)".format(tree.find("FileHeader").attrib.get('description', None)))
+ else:
+ tree = ET.parse(file_name)
+ for scenario in tree.iter("scenario"):
+ scenarios.append(scenario.attrib.get('name', None))
+
+ return scenarios
diff --git a/scenario_runner/srunner/utilities/code_check_and_formatting.sh b/scenario_runner/srunner/utilities/code_check_and_formatting.sh
new file mode 100644
index 0000000..7873a97
--- /dev/null
+++ b/scenario_runner/srunner/utilities/code_check_and_formatting.sh
@@ -0,0 +1,18 @@
+#!/bin/bash
+
+autopep8 scenario_runner.py --in-place --max-line-length=120
+autopep8 srunner/scenariomanager/*.py --in-place --max-line-length=120
+autopep8 srunner/scenariomanager/actorcontrols/*.py --in-place --max-line-length=120
+autopep8 srunner/scenariomanager/scenarioatomics/*.py --in-place --max-line-length=120
+autopep8 srunner/scenarios/*.py --in-place --max-line-length=120
+autopep8 srunner/autoagents/*.py --in-place --max-line-length=120
+autopep8 srunner/tools/*.py --in-place --max-line-length=120
+autopep8 srunner/scenarioconfigs/*.py --in-place --max-line-length=120
+
+
+pylint --rcfile=.pylintrc --disable=I srunner/scenariomanager/
+pylint --rcfile=.pylintrc srunner/scenarios/
+pylint --rcfile=.pylintrc srunner/autoagents/
+pylint --rcfile=.pylintrc srunner/tools/
+pylint --rcfile=.pylintrc srunner/scenarioconfigs/
+pylint --rcfile=.pylintrc scenario_runner.py