From 70426c198cc2c8ffa7a070445d6f2a9ac9f665f5 Mon Sep 17 00:00:00 2001 From: synkrotron Date: Tue, 12 Sep 2023 14:08:01 +0800 Subject: [PATCH] Supporting several additional OpenScenario 1.0 tags (#1014) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update: more tags supported * fix bug of add new tags * Rename VehicleLateralDistance.xosc and add import * Update Changelog and openscenario_support for new tags * delete redundant collision_delay * fix: remove useless logs and add traffic signal parser --------- Co-authored-by: xiaofei Co-authored-by: jsc Co-authored-by: 肖飞 <13319202082@163.com> Co-authored-by: lei --- Docs/CHANGELOG.md | 7 +- Docs/openscenario_support.md | 8 +- srunner/examples/InitAddEntityAction.xosc | 294 +++++++++++++++ srunner/examples/InitDeleteEntityAction.xosc | 287 +++++++++++++++ srunner/examples/LaneOffsetActionExample.xosc | 227 ++++++++++++ srunner/examples/StoryAddEntityAction.xosc | 324 +++++++++++++++++ srunner/examples/StoryDeleteEntityAction.xosc | 340 ++++++++++++++++++ srunner/examples/SyncArrivalIntersection.xosc | 256 +++++++++++++ srunner/examples/VehicleLateralDistance.xosc | 261 ++++++++++++++ .../openscenario_configuration.py | 10 +- .../actorcontrols/npc_vehicle_control.py | 2 + .../scenariomanager/carla_data_provider.py | 2 + .../scenarioatomics/atomic_behaviors.py | 289 +++++++++++++++ .../atomic_trigger_conditions.py | 67 ++++ srunner/scenarios/open_scenario.py | 130 ++++++- srunner/tools/openscenario_parser.py | 149 +++++++- 16 files changed, 2629 insertions(+), 24 deletions(-) create mode 100644 srunner/examples/InitAddEntityAction.xosc create mode 100644 srunner/examples/InitDeleteEntityAction.xosc create mode 100644 srunner/examples/LaneOffsetActionExample.xosc create mode 100644 srunner/examples/StoryAddEntityAction.xosc create mode 100644 srunner/examples/StoryDeleteEntityAction.xosc create mode 100644 srunner/examples/SyncArrivalIntersection.xosc create mode 100644 srunner/examples/VehicleLateralDistance.xosc diff --git a/Docs/CHANGELOG.md b/Docs/CHANGELOG.md index f0dbcfecf..16992ad31 100644 --- a/Docs/CHANGELOG.md +++ b/Docs/CHANGELOG.md @@ -18,7 +18,12 @@ * SignalizedJunctionLeftTurn has been remade. It now has an actor flow on which the ego has to merge into, instead of a single vehicle. * The BackgroundActivity has been readded to the routes, which the objective of creating the sensation of traffic around the ego * Add waypoint reached threshold so that the precision of the actor reaching to waypoints can be adjusted based on object types. -* **Supported OpenSCENARIO 2.0 standard.** +* OpenSCENARIO support: + - Added both init and story support for `EntityAction` + - Added story support for `TrafficSignalControllerAction` + - Added both init and story support for `LateralDistanceAction` + - Added support for `TrafficSignalControllerCondition` +* Supported OpenSCENARIO 2.0 standard. ### :bug: Bug Fixes * Fixed bug at OtherLeadingVehicle scenario causing the vehicles to move faster than intended diff --git a/Docs/openscenario_support.md b/Docs/openscenario_support.md index eab05f23b..ea98b5592 100755 --- a/Docs/openscenario_support.md +++ b/Docs/openscenario_support.md @@ -67,11 +67,11 @@ contains of submodules, which are not listed, the support status applies to all | GlobalAction | Init support | Story support | Notes | | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- | ---------------------------------------- | -| `EntityAction` | ❌ | ❌ | | +| `EntityAction` | ✅ | ✅ | | | `EnvironmentAction` | ✅ | ❌ | | | `ParameterAction` | ✅ | ✅ | | | `InfrastructureAction` `TrafficSignalAction`
`TrafficAction` | ❌ | ❌ | | -| `InfrastructureAction` `TrafficSignalAction`
`TrafficSignalControllerAction` | ❌ | ❌ | | +| `InfrastructureAction` `TrafficSignalAction`
`TrafficSignalControllerAction` | ❌ | ✅ | | | `InfrastructureAction` `TrafficSignalAction`
`TrafficSignalStateAction` | ❌ | ✅ | As traffic signals in CARLA towns have no unique ID, they have to be set by providing their position (Example: TrafficSignalStateAction name="pos=x,y" state="green"). The id can also be used for none CARLA town (Example: TrafficSignalStateAction name="id=n" state="green") |
@@ -98,7 +98,7 @@ contains of submodules, which are not listed, the support status applies to all | `ControllerAction` | ✅ | ✅ | AssignControllerAction is supported, but a Python module has to be provided for the controller implementation, and in OverrideControllerValueAction all values need to be `False`. | | `LateralAction`
`LaneChangeAction` | ❌ | ✅ | Currently all lane changes have a linear dynamicShape, the dynamicDimension is defined as the distance and are relative to the actor itself (RelativeTargetLane). | | `LateralAction`
`LaneOffsetAction` | ❌ | ✅ | Currently all type of dynamicShapes are ignored and depend on the controller. This action might not work as intended if the offset is high enough to make the vehicle exit its lane | -| `LateralAction`
`LateralDistanceAction` | ❌ | ❌ | | +| `LateralAction`
`LateralDistanceAction` | ✅ | ✅ | | | `LongitudinalAction`
`LongitudinalDistanceAction`| ❌ | ✅ |`timeGap` attribute is not supported | | `LongitudinalAction`
`SpeedAction` | ✅ | ✅ | | | `SynchronizeAction` | ❌ | ❌ | | @@ -149,7 +149,7 @@ The following two tables list the support status for each. | `StoryboardElementStateCondition` | ✅ | startTransition, stopTransition, endTransition and completeState are currently supported. | | `TimeOfDayCondition` | ✅ | | | `TrafficSignalCondition` | ✅ | As traffic signals in CARLA towns have no unique ID, they have to be set by providing their position (Example: TrafficSignalCondition name="pos=x,y" state="green"). The id can also be used for none CARLA town (Example: TrafficSignalCondition name="id=n" state="green") | -| `TrafficSignalControllerCondition` | ❌ | | +| `TrafficSignalControllerCondition` | ✅ | | | `UserDefinedValueCondition` | ❌ | |
diff --git a/srunner/examples/InitAddEntityAction.xosc b/srunner/examples/InitAddEntityAction.xosc new file mode 100644 index 000000000..cfcbe82b5 --- /dev/null +++ b/srunner/examples/InitAddEntityAction.xosc @@ -0,0 +1,294 @@ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/InitDeleteEntityAction.xosc b/srunner/examples/InitDeleteEntityAction.xosc new file mode 100644 index 000000000..75c85e6aa --- /dev/null +++ b/srunner/examples/InitDeleteEntityAction.xosc @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/LaneOffsetActionExample.xosc b/srunner/examples/LaneOffsetActionExample.xosc new file mode 100644 index 000000000..7f9ab2f50 --- /dev/null +++ b/srunner/examples/LaneOffsetActionExample.xosc @@ -0,0 +1,227 @@ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/StoryAddEntityAction.xosc b/srunner/examples/StoryAddEntityAction.xosc new file mode 100644 index 000000000..e03a89705 --- /dev/null +++ b/srunner/examples/StoryAddEntityAction.xosc @@ -0,0 +1,324 @@ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/StoryDeleteEntityAction.xosc b/srunner/examples/StoryDeleteEntityAction.xosc new file mode 100644 index 000000000..33e1ac6bd --- /dev/null +++ b/srunner/examples/StoryDeleteEntityAction.xosc @@ -0,0 +1,340 @@ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/SyncArrivalIntersection.xosc b/srunner/examples/SyncArrivalIntersection.xosc new file mode 100644 index 000000000..aa9e8825f --- /dev/null +++ b/srunner/examples/SyncArrivalIntersection.xosc @@ -0,0 +1,256 @@ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/examples/VehicleLateralDistance.xosc b/srunner/examples/VehicleLateralDistance.xosc new file mode 100644 index 000000000..e08ba2970 --- /dev/null +++ b/srunner/examples/VehicleLateralDistance.xosc @@ -0,0 +1,261 @@ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srunner/scenarioconfigs/openscenario_configuration.py b/srunner/scenarioconfigs/openscenario_configuration.py index 4ecb64535..7bdbb0b29 100644 --- a/srunner/scenarioconfigs/openscenario_configuration.py +++ b/srunner/scenarioconfigs/openscenario_configuration.py @@ -92,6 +92,7 @@ def _parse_openscenario_configuration(self): self._set_carla_town() self._set_actor_information() + self._set_traffic_signal_controller() self._validate_result() def _check_version(self): @@ -372,7 +373,6 @@ def _get_actor_transform(self, actor_name): 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): @@ -415,3 +415,11 @@ def _validate_result(self): if not self.ego_vehicles: self.logger.warning(" No ego vehicles defined in scenario") + + def _set_traffic_signal_controller(self): + if self.xml_tree.find("RoadNetwork") is not None: + rnw = self.xml_tree.find("RoadNetwork") + controller = rnw.find("TrafficSignals") + if controller is not None: + OpenScenarioParser.set_traffic_signal_controller(controller) + \ No newline at end of file diff --git a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py index c90d02b2f..f6caebdca 100644 --- a/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py +++ b/srunner/scenariomanager/actorcontrols/npc_vehicle_control.py @@ -105,6 +105,8 @@ def run_step(self): raise NotImplementedError("Negative target speeds are not yet supported") self._local_planner.set_speed(target_speed * 3.6) + if not self._actor.is_alive: + return control = self._local_planner.run_step(debug=False) # Check if the actor reached the end of the plan diff --git a/srunner/scenariomanager/carla_data_provider.py b/srunner/scenariomanager/carla_data_provider.py index a5037ad33..39ac61874 100644 --- a/srunner/scenariomanager/carla_data_provider.py +++ b/srunner/scenariomanager/carla_data_provider.py @@ -713,6 +713,8 @@ def request_new_actors(actor_list, safe_blueprint=False, tick=True): # Get the spawn point transform = actor.transform + if not transform: + continue if actor.random_location: if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): print("No more spawn points to use") diff --git a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py index a45ed8f69..bebb7fdb0 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_behaviors.py @@ -1276,6 +1276,187 @@ def terminate(self, new_status): super(ChangeActorLaneOffset, self).terminate(new_status) +class ChangeLateralDistance(AtomicBehavior): + """ + OpenSCENARIO atomic. + Atomic to change the offset of the controller. + + The behavior is in RUNNING state until the offset os reached (if 'continuous' is set to False) + or forever (if 'continuous' is True). This behavior will automatically stop if a second waypoint + related atomic for the same actor is triggered. These are: + - ChangeActorWaypoints + - ChangeActorLateralMotion + - ChangeActorLaneOffset + + Args: + actor (carla.Actor): Controlled actor. + offset (float): Float determined the distance to the center of the lane. Positive distance imply a + displacement to the right, while negative displacements are to the left. + relative_actor (carla.Actor): The actor from which the offset is taken from. Defaults to None + continuous (bool): If True, the behaviour never ends. If False, the behaviour ends when the lane + offset is reached. Defaults to True. + + Attributes: + _offset (float): lane offset. + _relative_actor (carla.Actor): relative actor. + _continuous (bool): stored the value of the 'continuous' argument. + _start_time (float): Start time of the atomic [s]. + Defaults to None. + _overwritten (bool): flag to check whether or not this behavior was overwritten by another. Helps + to avoid the missinteraction between two ChangeActorLaneOffsets. + _current_target_offset (float): stores the value of the offset when dealing with relative distances + _map (carla.Map): instance of the CARLA map. + """ + + OFFSET_THRESHOLD = 0.3 + + def __init__(self, actor, offset, relative_actor=None, freespace=False, + continuous=True, name="ChangeActorWaypoints", event_name=None): + """ + Setup parameters + """ + super(ChangeLateralDistance, self).__init__(name, actor) + + self._offset = offset + self._relative_actor = relative_actor + self._continuous = continuous + self._freespace = freespace + self._start_time = None + self._current_target_offset = 0 + self._overwritten = False + self._map = CarlaDataProvider.get_map() + if freespace: + if self._offset > 0: + self._offset += self._relative_actor.bounding_box.extent.y + self._actor.bounding_box.extent.y + else: + self._offset -= self._relative_actor.bounding_box.extent.y + self._actor.bounding_box.extent.y + self._actor_name = actor.attributes.get("role_name") + self._event_name = event_name + if relative_actor: + self._ref_actor_name = relative_actor.attributes.get("role_name") + + def initialise(self): + """ + Set _start_time and get (actor, controller) pair from Blackboard. + + Set offset 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 self._actor.id not in actor_dict: + raise RuntimeError("Actor not found in ActorsWithController BlackBoard") + + self._start_time = GameTime.get_time() + + actor_dict[self._actor.id].update_offset(self._offset, start_time=self._start_time) + + super(ChangeLateralDistance, self).initialise() + + def update(self): + """ + Check the actor's state along the waypoint route. + + returns: + py_trees.common.Status.SUCCESS, if the lane offset was reached (and 'continuous' was False), or + if another waypoint 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.RUNNING, else. + """ + try: + check_actors = operator.attrgetter("ActorsWithController") + actor_dict = check_actors(py_trees.blackboard.Blackboard()) + except AttributeError: + pass + + if not actor_dict or self._actor.id not in actor_dict: + return py_trees.common.Status.FAILURE + + if actor_dict[self._actor.id].get_last_lane_offset_command() != self._start_time: + # Differentiate between lane offset and other lateral commands + self._overwritten = True + # last_lane_offset_command_time = actor_dict[self._actor.id].get_last_lane_offset_command() + return py_trees.common.Status.SUCCESS + + if actor_dict[self._actor.id].get_last_waypoint_command() != self._start_time: + # last_waypoint_command_time = actor_dict[self._actor.id].get_last_waypoint_command() + return py_trees.common.Status.SUCCESS + + if self._relative_actor: + # Calculate new offset + relative_actor_loc = CarlaDataProvider.get_location(self._relative_actor) + relative_center_wp = self._map.get_waypoint(relative_actor_loc) + + # Value + relative_center_loc = relative_center_wp.transform.location + relative_actor_offset = relative_actor_loc.distance(relative_center_loc) + + # Sign + f_vec = relative_center_wp.transform.get_forward_vector() + d_vec = relative_actor_loc - relative_center_loc + cross = f_vec.x * d_vec.y - f_vec.y * d_vec.x + + if cross < 0: + relative_actor_offset *= -1.0 + + self._current_target_offset = relative_actor_offset + self._offset + # Set the new offset + actor_dict[self._actor.id].update_offset(self._current_target_offset) + if not self._continuous: + # Calculate new offset + actor_loc = CarlaDataProvider.get_location(self._actor) + center_wp = self._map.get_waypoint(actor_loc) + + # Value + center_loc = center_wp.transform.location + actor_offset = actor_loc.distance(center_loc) + + # Sign + f_vec = center_wp.transform.get_forward_vector() + d_vec = actor_loc - center_loc + cross = f_vec.x * d_vec.y - f_vec.y * d_vec.x + + if cross < 0: + actor_offset *= -1.0 + # Check if the offset has been reached + if abs(actor_offset - self._current_target_offset) < self.OFFSET_THRESHOLD: + # reach_offset = abs(actor_offset - self._current_target_offset) + return py_trees.common.Status.SUCCESS + + # TODO: As their is no way to check the distance to a specific lane, both checks will fail if the + # actors are outside its 'route lane' or at an intersection + + new_status = py_trees.common.Status.RUNNING + return new_status + + def terminate(self, new_status): + """ + On termination of this behavior, the offset is set back to zero + """ + + if not self._overwritten: + try: + check_actors = operator.attrgetter("ActorsWithController") + actor_dict = check_actors(py_trees.blackboard.Blackboard()) + except AttributeError: + pass + + if actor_dict and self._actor.id in actor_dict: + actor_dict[self._actor.id].update_offset(0) + + self._overwritten = True + + super(ChangeLateralDistance, self).terminate(new_status) + + class ActorTransformSetterToOSCPosition(AtomicBehavior): """ @@ -2615,6 +2796,81 @@ def update(self): return new_status +class TrafficLightControllerSetter(AtomicBehavior): + """ + This class contains an atomic behavior to set the phase of a given traffic light controller + + Args: + actor (carla.TrafficLight): ID of the traffic light controller that shall be changed + state (carla.TrafficLightState): New target state + + """ + + def __init__(self, traffic_signal_id, state, duration, delay=None, ref_id=None, + name="TrafficLightControllerSetter"): + """ + Init + """ + super(TrafficLightControllerSetter, self).__init__(name) + self.actor_id = traffic_signal_id + self._actor = None + self._start_time = None + self.duration_time = None + self.timeout = float(duration) + self.delay = float(delay) if delay else None + self.ref_tl_id = ref_id + self._state = state + self._previous_traffic_light_info = {} + self.logger.debug("%s.__init__()" % self.__class__.__name__) + + def initialise(self): + self._start_time = GameTime.get_time() + self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id(self.actor_id) + if self._actor is None: + return py_trees.common.Status.FAILURE + + if self.ref_tl_id is not None and self.delay is not None: + elapsed_time = self._actor.get_elapsed_time() + self.duration_time = self.delay + self.timeout + elapsed_time + elif self.ref_tl_id is None and self.delay is None: + self.duration_time = self.timeout + else: + return py_trees.common.Status.FAILURE + self._previous_traffic_light_info[self._actor] = { + 'state': self._actor.get_state(), + 'green_time': self._actor.get_green_time(), + 'red_time': self._actor.get_red_time(), + 'yellow_time': self._actor.get_yellow_time() + } + self._actor.set_state(self._state) + self._actor.set_green_time(self.duration_time) + + def update(self): + """Waits until the adequate time has passed""" + + new_status = py_trees.common.Status.RUNNING + + if self._actor.is_alive: + if GameTime.get_time() - self._start_time > self.duration_time: + new_status = py_trees.common.Status.SUCCESS + return new_status + else: + return new_status + else: + # For some reason the actor is gone... + return py_trees.common.Status.FAILURE + + def terminate(self, new_status): + """Reset all traffic lights back to their previous states""" + if self._previous_traffic_light_info: + self._actor.set_state(self._previous_traffic_light_info[self._actor]['state']) + self._actor.set_green_time(self._previous_traffic_light_info[self._actor]['green_time']) + self._actor.set_red_time(self._previous_traffic_light_info[self._actor]['red_time']) + self._actor.set_yellow_time(self._previous_traffic_light_info[self._actor]['yellow_time']) + + super(TrafficLightControllerSetter, self).terminate(new_status) + + class ActorSource(AtomicBehavior): """ @@ -3377,3 +3633,36 @@ def update(self): self.logger.debug("%s.update()[%s->%s]" % (self.__class__.__name__, self.status, new_status)) return new_status + + +class AddActor(AtomicBehavior): + """ + Implementation for a behavior that will create a actor + at a given transform if no other actor exists in a given radius + from the transform. + + Important parameters: + - actor_type: Type of CARLA actors to be spawned + - transform: Spawn location + A parallel termination behavior has to be used. + """ + + def __init__(self, actor_type, transform, color=None, name="SpawnActor"): + """ + Setup class members + """ + super(AddActor, self).__init__(name) + self._actor_type = actor_type + self._spawn_point = transform + self._color = color + + def update(self): + new_status = py_trees.common.Status.RUNNING + try: + new_actor = CarlaDataProvider.request_new_actor( + self._actor_type, self._spawn_point, color=self._color) + if new_actor: + new_status = py_trees.common.Status.SUCCESS + except: # pylint: disable=bare-except + print("ActorSource unable to spawn actor") + return new_status diff --git a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py index 5754b8c72..34327ac66 100644 --- a/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py +++ b/srunner/scenariomanager/scenarioatomics/atomic_trigger_conditions.py @@ -1266,6 +1266,73 @@ def update(self): return new_status +class WaitForTrafficLightControllerState(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, traffic_signal_id, state, duration, delay=None, ref_id=None, + name="WaitForTrafficLightControllerState"): + """ + Init + """ + super(WaitForTrafficLightControllerState, self).__init__(name) + self.actor_id = traffic_signal_id + self._actor = None + self._start_time = None + self.duration_time = None + self.timeout = float(duration) + self.delay = float(delay) if delay else None + self.ref_tl_id = ref_id + self._state = state + self.logger.debug("%s.__init__()" % self.__class__.__name__) + + def initialise(self): + + self._actor = CarlaDataProvider.get_world().get_traffic_light_from_opendrive_id(self.actor_id) + if self._actor is None: + return py_trees.common.Status.FAILURE + + if self.ref_tl_id is not None and self.delay is not None: + group_tl = self._actor.get_group_traffic_lights() + traffic_lights_id_list = [target_tl.id for target_tl in group_tl] + if self._actor.id not in traffic_lights_id_list: + return py_trees.common.Status.FAILURE + elapsed_time = self._actor.get_elapsed_time() + self.duration_time = self.delay + self.timeout + elapsed_time + elif self.ref_tl_id is None and self.delay is None: + self.duration_time = self.timeout + else: + return py_trees.common.Status.FAILURE + + 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: + if self._start_time is None: + self._start_time = GameTime.get_time() + + if self._actor.state == self._state and GameTime.get_time() - self._start_time >= self.duration_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 WaitEndIntersection(AtomicCondition): """ diff --git a/srunner/scenarios/open_scenario.py b/srunner/scenarios/open_scenario.py index 4236d4425..2c04a8f70 100644 --- a/srunner/scenarios/open_scenario.py +++ b/srunner/scenarios/open_scenario.py @@ -11,11 +11,14 @@ from __future__ import print_function +from distutils.util import strtobool import itertools import os import py_trees -from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeWeather, ChangeRoadFriction, ChangeParameter +from srunner.scenariomanager.carla_data_provider import CarlaDataProvider +from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeWeather, ChangeRoadFriction, ChangeParameter, \ + ChangeActorLaneOffset, ChangeActorWaypoints, ChangeLateralDistance from srunner.scenariomanager.scenarioatomics.atomic_behaviors import ChangeActorControl, ChangeActorTargetSpeed from srunner.scenariomanager.timer import GameTime from srunner.scenarios.basic_scenario import BasicScenario @@ -240,6 +243,8 @@ def _create_init_behavior(self): init_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name="InitBehaviour") + actor_list = self.other_actors + self.ego_vehicles + [None] + for actor in self.config.other_actors + self.config.ego_vehicles: for carla_actor in self.other_actors + self.ego_vehicles: if (carla_actor is not None and 'role_name' in carla_actor.attributes and @@ -247,21 +252,99 @@ def _create_init_behavior(self): actor_init_behavior = py_trees.composites.Sequence(name="InitActor{}".format(actor.rolename)) controller_atomic = None - + 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, - scenario_file_path=os.path.dirname(self.config.filename)) + if private_action.find('ControllerAction') is not None: + 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, + scenario_file_path=os.path.dirname(self.config.filename)) + + elif private_action.find('LateralAction') is not None: + private_action = private_action.find('LateralAction') + if private_action.find('LaneOffsetAction') is not None: + lat_maneuver = private_action.find('LaneOffsetAction') + continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "true"))) + # Parsing of the different Dynamic shapes is missing + lane_target_offset = lat_maneuver.find('LaneOffsetTarget') + if lane_target_offset.find('AbsoluteTargetLaneOffset') is not None: + absolute_offset = ParameterRef( + lane_target_offset.find('AbsoluteTargetLaneOffset').attrib.get('value', + 0)) + atomic = ChangeActorLaneOffset( + carla_actor, absolute_offset, continuous=continuous, + name='LaneOffsetAction') + + elif lane_target_offset.find('RelativeTargetLaneOffset') is not None: + relative_target_offset = lane_target_offset.find('RelativeTargetLaneOffset') + relative_offset = ParameterRef( + relative_target_offset.attrib.get('value', 0)) + relative_actor_name = relative_target_offset.attrib.get('entityRef', None) + relative_actor = None + for _actor in actor_list: + if _actor is not None and 'role_name' in _actor.attributes: + if relative_actor_name == _actor.attributes['role_name']: + relative_actor = _actor + break + if relative_actor is None: + raise AttributeError( + "Cannot find actor '{}' for condition".format(relative_actor_name)) + atomic = ChangeActorLaneOffset(carla_actor, relative_offset, relative_actor, + continuous=continuous, + name='LaneOffsetAction') + if private_action.find("LateralDistanceAction") is not None: + lat_maneuver = private_action.find('LateralDistanceAction') + maneuver_name = "LateralDistanceActionInit" + continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "false"))) + freespace = bool(strtobool(lat_maneuver.attrib.get('freespace', "false"))) + distance = ParameterRef(lat_maneuver.attrib.get('distance', float("inf"))) + constraints = lat_maneuver.find('DynamicConstraints') + max_speed = constraints.attrib.get('maxSpeed', + None) if constraints is not None else None + relative_actor = None + relative_actor_name = lat_maneuver.attrib.get('entityRef', None) + for _actor in actor_list: + if _actor is not None and 'role_name' in _actor.attributes: + if relative_actor_name == _actor.attributes['role_name']: + relative_actor = _actor + break + if relative_actor is None: + raise AttributeError( + "Cannot find actor '{}' for condition".format(relative_actor_name)) + atomic = ChangeLateralDistance(carla_actor, distance, relative_actor, + continuous=continuous, freespace=freespace, + 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: + route_action = private_action.find('AssignRouteAction') + waypoints = OpenScenarioParser.get_route(route_action, self.config.catalogs) + atomic = ChangeActorWaypoints(carla_actor, waypoints=waypoints, + name="AssignRouteAction") + elif private_action.find('FollowTrajectoryAction') is not None: + trajectory_action = private_action.find('FollowTrajectoryAction') + waypoints, times = OpenScenarioParser.get_trajectory(trajectory_action, + self.config.catalogs) + atomic = ChangeActorWaypoints(carla_actor, waypoints=list( + zip(waypoints, ['shortest'] * len(waypoints))), + times=times, name="FollowTrajectoryAction") + elif private_action.find('AcquirePositionAction') is not None: + route_action = private_action.find('AcquirePositionAction') + osc_position = route_action.find('Position') + waypoints = [(osc_position, 'fastest')] + atomic = ChangeActorWaypoints(carla_actor, waypoints=waypoints, + name="AcquirePositionAction") if controller_atomic is None: controller_atomic = ChangeActorControl(carla_actor, control_py_module=None, args={}) - actor_init_behavior.add_child(controller_atomic) + if atomic is not None: + actor_init_behavior.add_child(atomic) if actor.speed > 0: actor_init_behavior.add_child(ChangeActorTargetSpeed(carla_actor, actor.speed, init_speed=True)) @@ -342,7 +425,7 @@ def _create_behavior(self): for actor_id in actor_ids: maneuver_behavior = OpenScenarioParser.convert_maneuver_to_atomic( child, joint_actor_list[actor_id], - joint_actor_list, self.config.catalogs) + joint_actor_list, self.config.catalogs, self.config) maneuver_behavior = StoryElementStatusToBlackboard( maneuver_behavior, "ACTION", child.attrib.get('name')) parallel_actions.add_child( @@ -498,3 +581,30 @@ def __del__(self): Remove all actors upon deletion """ self.remove_all_actors() + + def _initialize_actors(self, config): + """ + Override the superclass method to initialize other actors + """ + if config.other_actors: + for global_action in self.config.init.find("Actions").iter("GlobalAction"): + if global_action.find("EntityAction") is not None: + entity_action = global_action.find("EntityAction") + entity_ref = entity_action.attrib.get("entityRef") + if entity_action.find('AddEntityAction') is not None: + position = entity_action.find('AddEntityAction').find("Position") + actor_transform = OpenScenarioParser.convert_position_to_transform( + position, actor_list=config.other_actors + config.ego_vehicles) + for actor in config.other_actors: + if actor.rolename == entity_ref: + actor.transform = actor_transform + elif entity_action.find('DeleteEntityAction') is not None: + for actor in config.other_actors: + if actor.rolename == entity_ref: + config.other_actors.remove(actor) + + 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) diff --git a/srunner/tools/openscenario_parser.py b/srunner/tools/openscenario_parser.py index adc0f9ed7..a4437b5e4 100644 --- a/srunner/tools/openscenario_parser.py +++ b/srunner/tools/openscenario_parser.py @@ -37,7 +37,9 @@ SyncArrivalOSC, KeepLongitudinalGap, Idle, - ChangeParameter) + ChangeParameter, ChangeLateralDistance, + ActorDestroy, AddActor, + TrafficLightControllerSetter) # 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, @@ -68,7 +70,8 @@ TimeOfDayComparison, TriggerVelocity, WaitForTrafficLightState, - CheckParameter) + CheckParameter, + WaitForTrafficLightControllerState) from srunner.scenariomanager.timer import TimeOut, SimulationTimeCondition from srunner.tools.py_trees_port import oneshot_behavior from srunner.tools.scenario_helper import get_offset_transform, get_troad_from_transform @@ -247,6 +250,8 @@ class OpenScenarioParser(object): "OFF": carla.TrafficLightState.Off, } + osc_traffic_signal_phase = {} + osc_traffic_signal_controller = {} global_osc_parameters = {} use_carla_coordinate_system = False osc_filepath = None @@ -890,7 +895,6 @@ def convert_condition_to_atomic(condition, actor_list): 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: @@ -1036,13 +1040,31 @@ def convert_condition_to_atomic(condition, actor_list): 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') + # check relative position and find reference entity + if ((position.find('RelativeWorldPosition') is not None) or + (position.find('RelativeObjectPosition') is not None) or + (position.find('RelativeLanePosition') is not None) or + (position.find('RelativeRoadPosition') 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') + if position.find('RelativeRoadPosition') is not None: + rel_pos = position.find('RelativeRoadPosition') + for _actor in actor_list: + if rel_pos.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( + rel_pos.attrib.get('entityRef', None))) atomic = InTriggerDistanceToOSCPosition( trigger_actor, position, distance_value, distance_along_route, distance_operator, name=condition_name) @@ -1133,7 +1155,23 @@ def convert_condition_to_atomic(condition, actor_list): 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") + traffic_light_controller_condition = value_condition.find('TrafficSignalControllerCondition') + ref_name = traffic_light_controller_condition.attrib.get("trafficSignalControllerRef") + phase_name = traffic_light_controller_condition.attrib.get("phase") + + if ref_name == OpenScenarioParser.osc_traffic_signal_controller["name"]: + delay = OpenScenarioParser.osc_traffic_signal_controller["delay"] + ref_id = OpenScenarioParser.osc_traffic_signal_controller["ref"] + if phase_name == OpenScenarioParser.osc_traffic_signal_phase["name"]: + duration = OpenScenarioParser.osc_traffic_signal_phase["duration"] + tl_state = OpenScenarioParser.osc_traffic_signal_phase["state"] + traffic_signal_id = OpenScenarioParser.osc_traffic_signal_phase["traffic_signal_id"] + + if (phase_name == "go" and tl_state == "GREEN") or (phase_name == "stop" and tl_state == "RED") or ( + phase_name == "attention" and tl_state == "YELLOW"): + state = OpenScenarioParser.tl_states[tl_state] + atomic = WaitForTrafficLightControllerState(traffic_signal_id, state, duration, delay=delay, + ref_id=ref_id, name=condition_name) else: raise AttributeError("Unknown ByValue condition") @@ -1150,7 +1188,7 @@ def convert_condition_to_atomic(condition, actor_list): return new_atomic @staticmethod - def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): + def convert_maneuver_to_atomic(action, actor, actor_list, catalogs, config): """ Convert an OpenSCENARIO maneuver action into a Behavior atomic @@ -1175,6 +1213,27 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): atomic = TrafficLightStateSetter( traffic_light, traffic_light_state, name=maneuver_name + "_" + str(traffic_light.id)) + elif infrastructure_action.find('TrafficSignalControllerAction') is not None: + traffic_light_controller_action = infrastructure_action.find('TrafficSignalControllerAction') + + ref_name = traffic_light_controller_action.attrib.get("trafficSignalControllerRef") + phase_name = traffic_light_controller_action.attrib.get("phase") + + if ref_name == OpenScenarioParser.osc_traffic_signal_controller["name"]: + delay = OpenScenarioParser.osc_traffic_signal_controller["delay"] + ref_id = OpenScenarioParser.osc_traffic_signal_controller["ref"] + if phase_name == OpenScenarioParser.osc_traffic_signal_phase["name"]: + duration = OpenScenarioParser.osc_traffic_signal_phase["duration"] + tl_state = OpenScenarioParser.osc_traffic_signal_phase["state"] + traffic_signal_id = OpenScenarioParser.osc_traffic_signal_phase["traffic_signal_id"] + + if (phase_name == "go" and tl_state == "GREEN") or (phase_name == "stop" and tl_state == "RED") or ( + phase_name == "attention" and tl_state == "YELLOW"): + state = OpenScenarioParser.tl_states[tl_state] + atomic = TrafficLightControllerSetter(traffic_signal_id, state, duration, + delay=delay, ref_id=ref_id, name=maneuver_name) + else: + raise ValueError("ERROR：Phase and state mismatch.Please check.") else: raise NotImplementedError("TrafficLights can only be influenced via TrafficSignalStateAction") elif global_action.find('EnvironmentAction') is not None: @@ -1204,6 +1263,30 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): else: rule, value = None, parameter_action.find('SetAction').attrib.get('value') atomic = ChangeParameter(parameter_ref, value=ParameterRef(value), rule=rule, name=maneuver_name) + elif global_action.find('EntityAction') is not None: + entity_action = global_action.find('EntityAction') + entity_ref = entity_action.attrib.get('entityRef') + if entity_action.find('DeleteEntityAction') is not None: + entity_ref_actor = None + for _actor in actor_list: + if _actor is not None and 'role_name' in _actor.attributes: + if entity_ref == _actor.attributes['role_name']: + entity_ref_actor = _actor + break + if entity_ref_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(entity_ref_actor)) + atomic = ActorDestroy(entity_ref_actor) + elif entity_action.find('AddEntityAction') is not None: + position = entity_action.find('AddEntityAction').find("Position") + actor_transform = OpenScenarioParser.convert_position_to_transform(position) + entity_ref_actor = None + for _actor in config.other_actors: + if _actor.rolename == entity_ref: + entity_ref_actor = _actor + break + if entity_ref_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(entity_ref_actor)) + atomic = AddActor(entity_ref_actor.model, actor_transform, color=entity_ref_actor.color) else: raise NotImplementedError("Global actions are not yet supported") elif action.find('UserDefinedAction') is not None: @@ -1341,6 +1424,25 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): else: raise AttributeError("Unknown target offset") + elif private_action.find('LateralDistanceAction') is not None: + lat_maneuver = private_action.find('LateralDistanceAction') + continuous = bool(strtobool(lat_maneuver.attrib.get('continuous', "false"))) + freespace = bool(strtobool(lat_maneuver.attrib.get('freespace', "false"))) + distance = ParameterRef(lat_maneuver.attrib.get('distance', float("inf"))) + constraints = lat_maneuver.find('DynamicConstraints') + max_speed = constraints.attrib.get('maxSpeed', None) if constraints is not None else None + relative_actor = None + relative_actor_name = lat_maneuver.attrib.get('entityRef', None) + for _actor in actor_list: + if _actor is not None and 'role_name' in _actor.attributes: + if relative_actor_name == _actor.attributes['role_name']: + relative_actor = _actor + break + if relative_actor is None: + raise AttributeError("Cannot find actor '{}' for condition".format(relative_actor_name)) + atomic = ChangeLateralDistance(actor, distance, relative_actor, + continuous=continuous, freespace=freespace, + name=maneuver_name) else: raise AttributeError("Unknown lateral action") elif private_action.find('VisibilityAction') is not None: @@ -1421,3 +1523,34 @@ def convert_maneuver_to_atomic(action, actor, actor_list, catalogs): return Idle(duration=0, name=maneuver_name) return atomic + + @staticmethod + def set_traffic_signal_controller(controller): + """set traffic signal controller""" + tl_states_dict = {1: "RED", 2: "YELLOW", 3: "GREEN"} + + sl_state = None + ts_controler = controller.find("TrafficSignalController") + controller_name = ts_controler.attrib.get("name") + controller_delay = ts_controler.attrib.get("delay") + controller_reference = ts_controler.attrib.get("reference") + phase = ts_controler.find("Phase") + phase_name = phase.attrib.get("name") + phase_duration = phase.attrib.get("duration") + ts_states = phase.find("TrafficSignalState") + states = ts_states.attrib.get("state") + # state: off;off;on + states = list(states.split(";")) + for i, state in enumerate(states): + if state == "on": + sl_state = tl_states_dict[int(i + 1)] + traffic_signal_id = ts_states.attrib.get("trafficSignalId") + + OpenScenarioParser.osc_traffic_signal_controller["name"] = controller_name + OpenScenarioParser.osc_traffic_signal_controller["delay"] = controller_delay + OpenScenarioParser.osc_traffic_signal_controller["ref"] = controller_reference + + OpenScenarioParser.osc_traffic_signal_phase["name"] = phase_name + OpenScenarioParser.osc_traffic_signal_phase["duration"] = phase_duration + OpenScenarioParser.osc_traffic_signal_phase["state"] = sl_state + OpenScenarioParser.osc_traffic_signal_phase["traffic_signal_id"] = traffic_signal_id