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
