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