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_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_behavior(self): """ Basic behavior do nothing, i.e. Idle """ behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) behavior.add_child(self.master_scenario.scenario.behavior) subbehavior = py_trees.composites.Parallel( name="Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL) for i in range(len(self.list_scenarios)): scenario = self.list_scenarios[i] if scenario.scenario.behavior is not None and scenario.scenario.behavior.name != "MasterScenario": name = "{} - {}".format(i, scenario.scenario.behavior.name) oneshot_idiom = oneshot_behavior( name=name, variable_name=name, behaviour=scenario.scenario.behavior) subbehavior.add_child(oneshot_idiom) behavior.add_child(subbehavior) return behavior
def _create_condition_container(self, node, name='Conditions Group', oneshot=False): """ 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 If oneshot is set to True, oneshot_behaviour will be applied to conditions """ parallel_condition_groups = py_trees.composites.Parallel( name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) for condition_group in node.iter("ConditionGroup"): condition_group_sequence = py_trees.composites.Sequence( name="Condition Group") for condition in condition_group.iter("Condition"): criterion = OpenScenarioParser.convert_condition_to_atomic( condition, self.other_actors + self.ego_vehicles) if oneshot: criterion = oneshot_behavior( variable_name=get_py_tree_path(criterion), 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_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) behavior.add_child(self.master_scenario.scenario.behavior) 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 \ and scenario.scenario.behavior.name not in ("MasterScenario", "Sequence"): 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_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 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"): for k, _ in enumerate(joint_actor_list): if entity.attrib.get('entityRef', None) == joint_actor_list[k].attributes['role_name']: actor_ids.append(k) break if not actor_ids: print("Warning: Maneuvergroup does not use reference actors!") # 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 = self.config.catalogs[catalog_reference.attrib.get( "catalogName")][catalog_reference.attrib.get("entryName")] 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]) 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) + '>' + get_xml_path(maneuver, maneuver), # 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") init_behavior = self._create_init_behavior() if init_behavior is not None: behavior.add_child(oneshot_behavior(variable_name=get_xml_path( self.config.story, self.config.story), behaviour=init_behavior)) behavior.add_child(story_behavior) return behavior
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_id = None traffic_light_action = infrastructure_action.find('TrafficSignalStateAction') name = traffic_light_action.attrib.get('name') if name.startswith("id="): traffic_light_id = name[3:] elif name.startswith("pos="): position = name[4:] pos = position.split(",") for carla_actor in CarlaDataProvider.get_world().get_actors().filter('traffic.traffic_light'): carla_actor_loc = carla_actor.get_transform().location distance = carla_actor_loc.distance(carla.Location(x=float(pos[0]), y=float(pos[1]), z=carla_actor_loc.z)) if distance < 2.0: traffic_light_id = carla_actor.id break if traffic_light_id is None: raise AttributeError("Unknown traffic light {}".format(name)) traffic_light_state = traffic_light_action.attrib.get('state') atomic = TrafficLightStateSetter( traffic_light_id, 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 = UpdateWeather( OpenScenarioParser.get_weather_from_env_action(global_action, catalogs)) friction_behavior = UpdateRoadFriction( 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, 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 = KeepVelocity(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 = SetRelativeOSCVelocity(actor, obj_actor, value, value_type, continuous, duration, distance) 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 = LaneChange(actor, None, direction="left" if target_lane_rel < 0 else "right", distance_lane_change=distance, 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: raise NotImplementedError("Controller actions are not yet supported") 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: target_speed = 5.0 private_action = private_action.find('RoutingAction') if private_action.find('AssignRouteAction') is not None: private_action = private_action.find('AssignRouteAction') if private_action.find('Route') is not None: route = private_action.find('Route') plan = [] if route.find('ParameterDeclarations') is not None: if route.find('ParameterDeclarations').find('Parameter') is not None: parameter = route.find('ParameterDeclarations').find('Parameter') if parameter.attrib.get('name') == "Speed": target_speed = float(parameter.attrib.get('value', 5.0)) for waypoint in route.iter('Waypoint'): position = waypoint.find('Position') transform = OpenScenarioParser.convert_position_to_transform(position) plan.append(transform.location) atomic = WaypointFollower(actor, target_speed=target_speed, plan=plan, name=maneuver_name) elif private_action.find('CatalogReference') is not None: raise NotImplementedError("CatalogReference private actions are not yet supported") else: raise AttributeError("Unknown private FollowRoute action") 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: raise NotImplementedError("Private AcquirePosition actions are not yet supported") else: raise AttributeError("Unknown private routing action") else: raise AttributeError("Unknown private action") else: if action: raise AttributeError("Unknown action: {}".format(maneuver_name)) else: return Idle(duration=0, name=maneuver_name) return atomic
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 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("Sequence"): sequence_behavior = py_trees.composites.Sequence( name="Seq:" + sequence.attrib.get('name')) repetitions = sequence.attrib.get('numberOfExecutions', 1) actor_ids = [] for actor in sequence.iter("Actors"): for entity in actor.iter("Entity"): for k, _ in enumerate(joint_actor_list): if entity.attrib.get( 'name', None ) == joint_actor_list[k].attributes['role_name']: actor_ids.append(k) break single_sequence_iteration = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL, name=sequence.attrib.get('name')) for maneuver in sequence.iter("Maneuver"): 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]) maneuver_behavior = StoryElementStatusToBlackboard( maneuver_behavior, "ACTION", child.attrib.get('name')) parallel_actions.add_child( oneshot_behavior( variable_name=get_py_tree_path( maneuver_behavior), behaviour=maneuver_behavior)) if child.tag == "StartConditions": # There is always one StartConditions block per Event parallel_condition_groups = self._create_condition_container( child, "Parallel Condition Groups") 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_py_tree_path(event_sequence), behaviour=event_sequence)) maneuver_parallel = StoryElementStatusToBlackboard( maneuver_parallel, "MANEUVER", maneuver.attrib.get('name')) single_sequence_iteration.add_child( oneshot_behavior( variable_name=get_py_tree_path(maneuver_parallel), 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) for _ in range(int(repetitions)): sequence_behavior.add_child(single_sequence_iteration) if sequence_behavior.children: parallel_sequences.add_child( oneshot_behavior( variable_name=get_py_tree_path(sequence_behavior), behaviour=sequence_behavior)) if parallel_sequences.children: parallel_sequences = StoryElementStatusToBlackboard( parallel_sequences, "ACT", act.attrib.get('name')) parallel_behavior.add_child(parallel_sequences) for conditions in act.iter("Conditions"): for start_condition in conditions.iter("Start"): parallel_start_criteria = self._create_condition_container( start_condition, "StartConditions", oneshot=True) if parallel_start_criteria.children: start_conditions.add_child(parallel_start_criteria) for end_condition in conditions.iter("End"): parallel_end_criteria = self._create_condition_container( end_condition, "EndConditions") if parallel_end_criteria.children: parallel_behavior.add_child(parallel_end_criteria) for cancel_condition in conditions.iter("Cancel"): parallel_cancel_criteria = self._create_condition_container( cancel_condition, "CancelConditions") if parallel_cancel_criteria.children: parallel_behavior.add_child(parallel_cancel_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") init_behavior = self._create_init_behavior() if init_behavior is not None: behavior.add_child( oneshot_behavior(variable_name=get_py_tree_path(init_behavior), behaviour=init_behavior)) behavior.add_child(story_behavior) return behavior
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, 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, 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: private_action = private_action.find('AssignRouteAction') if private_action.find('Route') is not None: route = private_action.find('Route') waypoints = [] for waypoint in route.iter('Waypoint'): position = waypoint.find('Position') transform = OpenScenarioParser.convert_position_to_transform(position) waypoints.append(transform) # @TODO: How to handle relative positions here? This might chance at runtime?! atomic = ChangeActorWaypoints(actor, waypoints=waypoints, name=maneuver_name) elif private_action.find('CatalogReference') is not None: raise NotImplementedError("CatalogReference private actions are not yet supported") else: raise AttributeError("Unknown private FollowRoute action") 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: raise NotImplementedError("Private AcquirePosition actions are not yet supported") 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