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