def convert_condition_to_atomic(condition, actor_list):
"""
Convert an OpenSCENARIO condition into a Behavior/Criterion atomic
If there is a delay defined in the condition, then the condition is checked after the delay time
passed by, e.g. <Condition name="" delay="5">.
Note: Not all conditions are currently supported.
"""
atomic = None
delay_atomic = None
condition_name = condition.attrib.get('name')
if condition.attrib.get('delay') is not None and str(
condition.attrib.get('delay')) != '0':
delay = float(condition.attrib.get('delay'))
delay_atomic = TimeOut(delay)
if condition.find('ByEntity') is not None:
trigger_actor = None # A-priori validation ensures that this will be not None
triggered_actor = None
for triggering_entities in condition.find('ByEntity').iter(
'TriggeringEntities'):
for entity in triggering_entities.iter('Entity'):
for actor in actor_list:
if entity.attrib.get(
'name', None) == actor.attributes['role_name']:
trigger_actor = actor
break
for entity_condition in condition.find('ByEntity').iter(
'EntityCondition'):
if entity_condition.find('EndOfRoad') is not None:
raise NotImplementedError(
"EndOfRoad conditions are not yet supported")
elif entity_condition.find('Collision') is not None:
atomic = py_trees.meta.inverter(
CollisionTest(trigger_actor,
terminate_on_failure=True,
name=condition_name))
elif entity_condition.find('Offroad') is not None:
raise NotImplementedError(
"Offroad conditions are not yet supported")
elif entity_condition.find('TimeHeadway') is not None:
raise NotImplementedError(
"TimeHeadway conditions are not yet supported")
elif entity_condition.find('TimeToCollision') is not None:
ttc_condition = entity_condition.find('TimeToCollision')
condition_rule = ttc_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[
condition_rule]
condition_value = ttc_condition.attrib.get('value')
condition_target = ttc_condition.find('Target')
if condition_target.find('Position'):
position = condition_target.find('Position')
atomic = InTimeToArrivalToOSCPosition(
trigger_actor, position, condition_value,
condition_operator)
else:
for actor in actor_list:
if ttc_condition.attrib.get(
'entity',
None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError(
"Cannot find actor '{}' for condition".format(
ttc_condition.attrib.get('entity', None)))
atomic = InTimeToArrivalToVehicle(
trigger_actor, triggered_actor, condition_value,
condition_operator)
elif entity_condition.find('Acceleration') is not None:
raise NotImplementedError(
"Acceleration conditions are not yet supported")
elif entity_condition.find('StandStill') is not None:
ss_condition = entity_condition.find('StandStill')
duration = float(ss_condition.attrib.get('duration'))
atomic = StandStill(trigger_actor, condition_name,
duration)
elif entity_condition.find('Speed') is not None:
s_condition = entity_condition.find('Speed')
value = float(s_condition.attrib.get('value'))
if s_condition.attrib.get('rule') != "greater_than":
raise NotImplementedError(
"Speed condition with the given specification is not yet supported"
)
atomic = AccelerateToVelocity(trigger_actor, value,
condition_name)
elif entity_condition.find('RelativeSpeed') is not None:
raise NotImplementedError(
"RelativeSpeed conditions are not yet supported")
elif entity_condition.find('TraveledDistance') is not None:
distance_condition = entity_condition.find(
'TraveledDistance')
distance_value = float(
distance_condition.attrib.get('value'))
atomic = DriveDistance(trigger_actor,
distance_value,
name=condition_name)
elif entity_condition.find('ReachPosition') is not None:
rp_condition = entity_condition.find('ReachPosition')
distance_value = float(
rp_condition.attrib.get('tolerance'))
position = rp_condition.find('Position')
atomic = InTriggerDistanceToOSCPosition(
trigger_actor,
position,
distance_value,
name=condition_name)
elif entity_condition.find('Distance') is not None:
distance_condition = entity_condition.find('Distance')
distance_value = float(
distance_condition.attrib.get('value'))
distance_rule = distance_condition.attrib.get('rule')
distance_operator = OpenScenarioParser.operators[
distance_rule]
if distance_condition.find('Position') is not None:
position = distance_condition.find('Position')
atomic = InTriggerDistanceToOSCPosition(
trigger_actor,
position,
distance_value,
distance_operator,
name=condition_name)
elif entity_condition.find('RelativeDistance') is not None:
distance_condition = entity_condition.find(
'RelativeDistance')
distance_value = float(
distance_condition.attrib.get('value'))
if distance_condition.attrib.get('type') == "inertial":
for actor in actor_list:
if distance_condition.attrib.get(
'entity',
None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError(
"Cannot find actor '{}' for condition".format(
distance_condition.attrib.get(
'entity', None)))
condition_rule = distance_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[
condition_rule]
atomic = InTriggerDistanceToVehicle(
triggered_actor,
trigger_actor,
distance_value,
condition_operator,
name=condition_name)
else:
raise NotImplementedError(
"RelativeDistance condition with the given specification is not yet supported"
)
elif condition.find('ByState') is not None:
state_condition = condition.find('ByState')
if state_condition.find('AtStart') is not None:
element_type = state_condition.find('AtStart').attrib.get(
'type')
element_name = state_condition.find('AtStart').attrib.get(
'name')
atomic = OSCStartEndCondition(element_type,
element_name,
rule="START",
name="AtStartCondition")
elif state_condition.find('AfterTermination') is not None:
element_type = state_condition.find(
'AfterTermination').attrib.get('type')
element_name = state_condition.find(
'AfterTermination').attrib.get('name')
condition_rule = state_condition.find(
'AfterTermination').attrib.get('rule')
atomic = OSCStartEndCondition(element_type,
element_name,
condition_rule,
name="AfterTerminationCondition")
elif state_condition.find('Command') is not None:
raise NotImplementedError(
"ByState Command conditions are not yet supported")
elif state_condition.find('Signal') is not None:
raise NotImplementedError(
"ByState Signal conditions are not yet supported")
elif state_condition.find('Controller') is not None:
raise NotImplementedError(
"ByState Controller conditions are not yet supported")
else:
raise AttributeError("Unknown ByState condition")
elif condition.find('ByValue') is not None:
value_condition = condition.find('ByValue')
if value_condition.find('Parameter') is not None:
parameter_condition = value_condition.find('Parameter')
arg_name = parameter_condition.attrib.get('name')
value = parameter_condition.attrib.get('value')
if value != '':
arg_value = float(value)
else:
arg_value = 0
parameter_condition.attrib.get('rule')
if condition_name in globals():
criterion_instance = globals()[condition_name]
else:
raise AttributeError(
"The condition {} cannot be mapped to a criterion atomic"
.format(condition_name))
for triggered_actor in actor_list:
if arg_name != '':
atomic = criterion_instance(triggered_actor, arg_value)
else:
atomic = criterion_instance(triggered_actor)
elif value_condition.find('SimulationTime') is not None:
simtime_condition = value_condition.find('SimulationTime')
value = float(simtime_condition.attrib.get('value'))
rule = simtime_condition.attrib.get('rule')
atomic = SimulationTimeCondition(value, success_rule=rule)
elif value_condition.find('TimeOfDay') is not None:
raise NotImplementedError(
"ByValue TimeOfDay conditions are not yet supported")
else:
raise AttributeError("Unknown ByValue condition")
else:
raise AttributeError("Unknown condition")
if delay_atomic is not None and atomic is not None:
new_atomic = py_trees.composites.Sequence("delayed sequence")
new_atomic.add_child(delay_atomic)
new_atomic.add_child(atomic)
else:
new_atomic = atomic
return new_atomic
def convert_condition_to_atomic(condition, actor_list):
"""
Convert an OpenSCENARIO condition into a Behavior/Criterion atomic
If there is a delay defined in the condition, then the condition is checked after the delay time
passed by, e.g. <Condition name="" delay="5">.
Note: Not all conditions are currently supported.
"""
atomic = None
delay_atomic = None
condition_name = condition.attrib.get('name')
if condition.attrib.get('delay') is not None and str(condition.attrib.get('delay')) != '0':
delay = float(condition.attrib.get('delay'))
delay_atomic = TimeOut(delay)
if condition.find('ByEntityCondition') is not None:
trigger_actor = None # A-priori validation ensures that this will be not None
triggered_actor = None
for triggering_entities in condition.find('ByEntityCondition').iter('TriggeringEntities'):
for entity in triggering_entities.iter('EntityRef'):
for actor in actor_list:
if entity.attrib.get('entityRef', None) == actor.attributes['role_name']:
trigger_actor = actor
break
for entity_condition in condition.find('ByEntityCondition').iter('EntityCondition'):
if entity_condition.find('EndOfRoadCondition') is not None:
end_road_condition = entity_condition.find('EndOfRoadCondition')
condition_duration = float(end_road_condition.attrib.get('duration'))
atomic_cls = py_trees.meta.inverter(EndofRoadTest)
atomic = atomic_cls(
trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name)
elif entity_condition.find('CollisionCondition') is not None:
collision_condition = entity_condition.find('CollisionCondition')
if collision_condition.find('EntityRef') is not None:
collision_entity = collision_condition.find('EntityRef')
for actor in actor_list:
if collision_entity.attrib.get('entityRef', None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
collision_condition.attrib.get('entityRef', None)))
atomic_cls = py_trees.meta.inverter(CollisionTest)
atomic = atomic_cls(trigger_actor, other_actor=triggered_actor,
terminate_on_failure=True, name=condition_name)
elif collision_condition.find('ByType') is not None:
collision_type = collision_condition.find('ByType').attrib.get('type', None)
triggered_type = OpenScenarioParser.actor_types[collision_type]
atomic_cls = py_trees.meta.inverter(CollisionTest)
atomic = atomic_cls(trigger_actor, other_actor_type=triggered_type,
terminate_on_failure=True, name=condition_name)
else:
atomic_cls = py_trees.meta.inverter(CollisionTest)
atomic = atomic_cls(trigger_actor, terminate_on_failure=True, name=condition_name)
elif entity_condition.find('OffroadCondition') is not None:
off_condition = entity_condition.find('OffroadCondition')
condition_duration = float(off_condition.attrib.get('duration'))
atomic_cls = py_trees.meta.inverter(OffRoadTest)
atomic = atomic_cls(
trigger_actor, condition_duration, terminate_on_failure=True, name=condition_name)
elif entity_condition.find('TimeHeadwayCondition') is not None:
headtime_condition = entity_condition.find('TimeHeadwayCondition')
condition_value = float(headtime_condition.attrib.get('value'))
condition_rule = headtime_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
condition_freespace = strtobool(headtime_condition.attrib.get('freespace', False))
if condition_freespace:
raise NotImplementedError(
"TimeHeadwayCondition: freespace attribute is currently not implemented")
condition_along_route = strtobool(headtime_condition.attrib.get('alongRoute', False))
for actor in actor_list:
if headtime_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
headtime_condition.attrib.get('entityRef', None)))
atomic = InTimeToArrivalToVehicle(
trigger_actor, triggered_actor, condition_value,
condition_along_route, condition_operator, condition_name
)
elif entity_condition.find('TimeToCollisionCondition') is not None:
ttc_condition = entity_condition.find('TimeToCollisionCondition')
condition_rule = ttc_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
condition_value = ttc_condition.attrib.get('value')
condition_target = ttc_condition.find('TimeToCollisionConditionTarget')
condition_freespace = strtobool(ttc_condition.attrib.get('freespace', False))
if condition_freespace:
raise NotImplementedError(
"TimeToCollisionCondition: freespace attribute is currently not implemented")
condition_along_route = strtobool(ttc_condition.attrib.get('alongRoute', False))
if condition_target.find('Position') is not None:
position = condition_target.find('Position')
atomic = InTimeToArrivalToOSCPosition(
trigger_actor, position, condition_value, condition_along_route, condition_operator)
else:
for actor in actor_list:
if ttc_condition.attrib.get('EntityRef', None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
ttc_condition.attrib.get('EntityRef', None)))
atomic = InTimeToArrivalToVehicle(
trigger_actor, triggered_actor, condition_value,
condition_along_route, condition_operator, condition_name)
elif entity_condition.find('AccelerationCondition') is not None:
accel_condition = entity_condition.find('AccelerationCondition')
condition_value = float(accel_condition.attrib.get('value'))
condition_rule = accel_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
atomic = TriggerAcceleration(
trigger_actor, condition_value, condition_operator, condition_name)
elif entity_condition.find('StandStillCondition') is not None:
ss_condition = entity_condition.find('StandStillCondition')
duration = float(ss_condition.attrib.get('duration'))
atomic = StandStill(trigger_actor, condition_name, duration)
elif entity_condition.find('SpeedCondition') is not None:
spd_condition = entity_condition.find('SpeedCondition')
condition_value = float(spd_condition.attrib.get('value'))
condition_rule = spd_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
atomic = TriggerVelocity(
trigger_actor, condition_value, condition_operator, condition_name)
elif entity_condition.find('RelativeSpeedCondition') is not None:
relspd_condition = entity_condition.find('RelativeSpeedCondition')
condition_value = float(relspd_condition.attrib.get('value'))
condition_rule = relspd_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
for actor in actor_list:
if relspd_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
relspd_condition.attrib.get('entityRef', None)))
atomic = RelativeVelocityToOtherActor(
trigger_actor, triggered_actor, condition_value, condition_operator, condition_name)
elif entity_condition.find('TraveledDistanceCondition') is not None:
distance_condition = entity_condition.find('TraveledDistanceCondition')
distance_value = float(distance_condition.attrib.get('value'))
atomic = DriveDistance(trigger_actor, distance_value, name=condition_name)
elif entity_condition.find('ReachPositionCondition') is not None:
rp_condition = entity_condition.find('ReachPositionCondition')
distance_value = float(rp_condition.attrib.get('tolerance'))
position = rp_condition.find('Position')
atomic = InTriggerDistanceToOSCPosition(
trigger_actor, position, distance_value, name=condition_name)
elif entity_condition.find('DistanceCondition') is not None:
distance_condition = entity_condition.find('DistanceCondition')
distance_value = float(distance_condition.attrib.get('value'))
distance_rule = distance_condition.attrib.get('rule')
distance_operator = OpenScenarioParser.operators[distance_rule]
distance_freespace = strtobool(distance_condition.attrib.get('freespace', False))
if distance_freespace:
raise NotImplementedError(
"DistanceCondition: freespace attribute is currently not implemented")
distance_along_route = strtobool(distance_condition.attrib.get('alongRoute', False))
if distance_condition.find('Position') is not None:
position = distance_condition.find('Position')
atomic = InTriggerDistanceToOSCPosition(
trigger_actor, position, distance_value, distance_along_route,
distance_operator, name=condition_name)
elif entity_condition.find('RelativeDistanceCondition') is not None:
distance_condition = entity_condition.find('RelativeDistanceCondition')
distance_value = float(distance_condition.attrib.get('value'))
distance_freespace = strtobool(distance_condition.attrib.get('freespace', False))
if distance_freespace:
raise NotImplementedError(
"RelativeDistanceCondition: freespace attribute is currently not implemented")
if distance_condition.attrib.get('relativeDistanceType') == "cartesianDistance":
for actor in actor_list:
if distance_condition.attrib.get('entityRef', None) == actor.attributes['role_name']:
triggered_actor = actor
break
if triggered_actor is None:
raise AttributeError("Cannot find actor '{}' for condition".format(
distance_condition.attrib.get('entityRef', None)))
condition_rule = distance_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
atomic = InTriggerDistanceToVehicle(
triggered_actor, trigger_actor, distance_value, condition_operator, name=condition_name)
else:
raise NotImplementedError(
"RelativeDistance condition with the given specification is not yet supported")
elif condition.find('ByValueCondition') is not None:
value_condition = condition.find('ByValueCondition')
if value_condition.find('ParameterCondition') is not None:
parameter_condition = value_condition.find('ParameterCondition')
arg_name = parameter_condition.attrib.get('parameterRef')
value = parameter_condition.attrib.get('value')
if value != '':
arg_value = float(value)
else:
arg_value = 0
parameter_condition.attrib.get('rule')
if condition_name in globals():
criterion_instance = globals()[condition_name]
else:
raise AttributeError(
"The condition {} cannot be mapped to a criterion atomic".format(condition_name))
atomic = py_trees.composites.Parallel("Evaluation Criteria for multiple ego vehicles")
for triggered_actor in actor_list:
if arg_name != '':
atomic.add_child(criterion_instance(triggered_actor, arg_value))
else:
atomic.add_child(criterion_instance(triggered_actor))
elif value_condition.find('SimulationTimeCondition') is not None:
simtime_condition = value_condition.find('SimulationTimeCondition')
value = float(simtime_condition.attrib.get('value'))
rule = simtime_condition.attrib.get('rule')
atomic = SimulationTimeCondition(value, success_rule=rule)
elif value_condition.find('TimeOfDayCondition') is not None:
tod_condition = value_condition.find('TimeOfDayCondition')
condition_date = tod_condition.attrib.get('dateTime')
condition_rule = tod_condition.attrib.get('rule')
condition_operator = OpenScenarioParser.operators[condition_rule]
atomic = TimeOfDayComparison(condition_date, condition_operator, condition_name)
elif value_condition.find('StoryboardElementStateCondition') is not None:
state_condition = value_condition.find('StoryboardElementStateCondition')
element_name = state_condition.attrib.get('storyboardElementRef')
element_type = state_condition.attrib.get('storyboardElementType')
state = state_condition.attrib.get('state')
if state == "startTransition":
atomic = OSCStartEndCondition(element_type, element_name, rule="START", name=state + "Condition")
elif state == "stopTransition" or state == "endTransition" or state == "completeState":
atomic = OSCStartEndCondition(element_type, element_name, rule="END", name=state + "Condition")
else:
raise NotImplementedError(
"Only start, stop, endTransitions and completeState are currently supported")
elif value_condition.find('UserDefinedValueCondition') is not None:
raise NotImplementedError("ByValue UserDefinedValue conditions are not yet supported")
elif value_condition.find('TrafficSignalCondition') is not None:
tl_condition = value_condition.find('TrafficSignalCondition')
name_condition = tl_condition.attrib.get('name')
traffic_light = OpenScenarioParser.get_traffic_light_from_osc_name(name_condition)
tl_state = tl_condition.attrib.get('state').upper()
if tl_state not in OpenScenarioParser.tl_states:
raise KeyError("CARLA only supports Green, Red, Yellow or Off")
state_condition = OpenScenarioParser.tl_states[tl_state]
atomic = WaitForTrafficLightState(
traffic_light, state_condition, name=condition_name)
elif value_condition.find('TrafficSignalControllerCondition') is not None:
raise NotImplementedError("ByValue TrafficSignalController conditions are not yet supported")
else:
raise AttributeError("Unknown ByValue condition")
else:
raise AttributeError("Unknown condition")
if delay_atomic is not None and atomic is not None:
new_atomic = py_trees.composites.Sequence("delayed sequence")
new_atomic.add_child(delay_atomic)
new_atomic.add_child(atomic)
else:
new_atomic = atomic
return new_atomic