def _create_behavior(self):
"""
The scenario defined after is a "follow leading vehicle" scenario. After
invoking this scenario, it will wait for the user controlled vehicle to
enter the start region, then make the other actor to drive towards obstacle.
Once obstacle clears the road, make the other actor to drive towards the
next intersection. Finally, the user-controlled vehicle has to be close
enough to the other actor to end the scenario.
If this does not happen within 60 seconds, a timeout stops the scenario
"""
# let the other actor drive until next intersection
driving_to_next_intersection = py_trees.composites.Parallel(
"Driving towards Intersection",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
obstacle_clear_road = py_trees.composites.Parallel("Obstalce clearing road",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
obstacle_clear_road.add_child(DriveDistance(self.other_actors[1], 4))
obstacle_clear_road.add_child(KeepVelocity(self.other_actors[1], self._second_actor_speed))
stop_near_intersection = py_trees.composites.Parallel(
"Waiting for end position near Intersection",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
stop_near_intersection.add_child(WaypointFollower(self.other_actors[0], 10))
stop_near_intersection.add_child(InTriggerDistanceToNextIntersection(self.other_actors[0], 20))
driving_to_next_intersection.add_child(WaypointFollower(self.other_actors[0], self._first_actor_speed))
driving_to_next_intersection.add_child(InTriggerDistanceToVehicle(self.other_actors[1],
self.other_actors[0], 15))
# end condition
endcondition = py_trees.composites.Parallel("Waiting for end position",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
self.ego_vehicles[0],
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self._first_actor_transform))
sequence.add_child(ActorTransformSetter(self.other_actors[1], self._second_actor_transform))
sequence.add_child(driving_to_next_intersection)
sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
sequence.add_child(TimeOut(3))
sequence.add_child(obstacle_clear_road)
sequence.add_child(stop_near_intersection)
sequence.add_child(StopVehicle(self.other_actors[0], self._other_actor_max_brake))
sequence.add_child(endcondition)
sequence.add_child(ActorDestroy(self.other_actors[0]))
sequence.add_child(ActorDestroy(self.other_actors[1]))
return sequence
def _create_behavior(self):
"""
The scenario defined after is a "follow leading vehicle" scenario. After
invoking this scenario, it will wait for the user controlled vehicle to
enter the start region, then make the other actor to drive until reaching
the next intersection. Finally, the user-controlled vehicle has to be close
enough to the other actor to end the scenario.
If this does not happen within 60 seconds, a timeout stops the scenario
"""
# to avoid the other actor blocking traffic, it was spawed elsewhere
# reset its pose to the required one
start_transform = ActorTransformSetter(self.other_actors[0],
self._other_actor_transform)
# let the other actor drive until next intersection
# @todo: We should add some feedback mechanism to respond to ego_vehicle behavior
driving_to_next_intersection = py_trees.composites.Parallel(
"DrivingTowardsIntersection",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
driving_to_next_intersection.add_child(
WaypointFollower(self.other_actors[0], self._first_vehicle_speed))
driving_to_next_intersection.add_child(
InTriggerDistanceToNextIntersection(
self.other_actors[0],
self._other_actor_stop_in_front_intersection))
# stop vehicle
stop = StopVehicle(self.other_actors[0], self._other_actor_max_brake)
# end condition
endcondition = py_trees.composites.Parallel(
"Waiting for end position",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
self.ego_vehicles[0],
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0],
name="StandStill",
duration=1)
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(start_transform)
sequence.add_child(driving_to_next_intersection)
sequence.add_child(stop)
sequence.add_child(endcondition)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
# sequence vw
# make visible
sequence_vw = py_trees.composites.Sequence("VW T2")
vw_visible = ActorTransformSetter(self.other_actors[1], self.slow_car_visible)
sequence_vw.add_child(vw_visible)
# brake, avoid rolling backwarts
brake = StopVehicle(self.other_actors[1], self._max_brake)
sequence_vw.add_child(brake)
sequence_vw.add_child(Idle())
# sequence tesla
# make visible
sequence_tesla = py_trees.composites.Sequence("Tesla")
tesla_visible = ActorTransformSetter(self.other_actors[0], self.fast_car_visible)
sequence_tesla.add_child(tesla_visible)
# drive fast towards slow vehicle
just_drive = py_trees.composites.Parallel("DrivingTowardsSlowVehicle",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
tesla_driving_fast = WaypointFollower(self.other_actors[0], self._fast_vehicle_velocity)
just_drive.add_child(tesla_driving_fast)
distance_to_vehicle = InTriggerDistanceToVehicle(
self.other_actors[1], self.other_actors[0], self._trigger_distance)
just_drive.add_child(distance_to_vehicle)
sequence_tesla.add_child(just_drive)
# change lane
lane_change_atomic = LaneChange(self.other_actors[0], distance_other_lane=200)
sequence_tesla.add_child(lane_change_atomic)
sequence_tesla.add_child(Idle())
# ego vehicle
# end condition
endcondition = py_trees.composites.Parallel("Waiting for end position of ego vehicle",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[1],
self.ego_vehicles[0],
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
# build tree
root = py_trees.composites.Parallel("Parallel Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(sequence_vw)
root.add_child(sequence_tesla)
root.add_child(endcondition)
return root
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
def _create_behavior(self):
"""
The scenario defined after is a "follow leading vehicle" scenario. After
invoking this scenario, it will wait for the user controlled vehicle to
enter the start region, then make the other actor to drive until reaching
the next intersection. Finally, the user-controlled vehicle has to be close
enough to the other actor to end the scenario.
If this does not happen within 60 seconds, a timeout stops the scenario
"""
root = py_trees.composites.Parallel(
"Running random scenario",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(StandStill(self.ego_vehicles[0], "scenario ended", 3.0))
root.add_child(
InTriggerDistanceToLocation(self.ego_vehicles[0],
carla.Location(200, -250, 0), 10))
for (scenario, actor_dict) in self.scenario_list:
if scenario == "VehiclesAhead":
wait_for_trigger = InTriggerDistanceToLocation(
self.ego_vehicles[0],
actor_dict.trigger.transform.location, 10)
start_transform = ActorTransformSetter(
self.other_actors[actor_dict.index],
actor_dict.start_transform)
keep_driving = WaypointFollower(
self.other_actors[actor_dict.index],
actor_dict.speed,
avoid_collision=True)
drive = py_trees.composites.Parallel(
"Driving for a distance",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
drive_success = DriveDistance(
self.other_actors[actor_dict.index],
random.randint(50, 100))
drive.add_child(drive_success)
drive.add_child(keep_driving)
stand = StandStill(self.other_actors[actor_dict.index],
"stand still", random.randint(1, 5))
stop = StopVehicle(self.other_actors[actor_dict.index], 1.0)
accelerate = AccelerateToVelocity(
self.other_actors[actor_dict.index], 1.0, actor_dict.speed)
# Build behavior tree
sequence = py_trees.composites.Sequence(
"VehiclesAhead behavior sequence")
sequence.add_child(wait_for_trigger)
sequence.add_child(start_transform)
# stop_condition = py_trees.composites.Parallel("stop condition",
# policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# stop_condition.add_child(keep_driving)
# stop_condition.add_child(drive_success)
if actor_dict.stop:
sequence.add_child(drive)
sequence.add_child(stop)
sequence.add_child(accelerate)
sequence.add_child(keep_driving) #stop_condition
print("lead vehicle stop behavior added")
else:
sequence.add_child(keep_driving) #stop_condition
sequence.add_child(
ActorDestroy(self.other_actors[actor_dict.index]))
root.add_child(sequence)
elif scenario == "StationaryObstaclesAhead":
lane_width = self.ego_vehicles[0].get_world().get_map(
).get_waypoint(self.ego_vehicles[0].get_location()).lane_width
lane_width = lane_width + (1.25 * lane_width)
sequence = py_trees.composites.Sequence(
"StationaryObstaclesAhead behavior sequence")
# actor_stand = TimeOut(15)
# actor_removed = ActorDestroy(self.other_actors[num_actors])
# end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven)
sequence.add_child(
ActorTransformSetter(self.other_actors[actor_dict.index],
actor_dict.start_transform))
sequence.add_child(TimeOut(120))
root.add_child(sequence)
elif scenario == "CutIn":
sequence = py_trees.composites.Sequence("CarOn_{}_Lane".format(
actor_dict.direction))
car_visible = ActorTransformSetter(
self.other_actors[actor_dict.index],
actor_dict.start_transform)
sequence.add_child(
InTriggerDistanceToLocation(
self.ego_vehicles[0],
actor_dict.trigger.transform.location, 10))
sequence.add_child(car_visible)
# accelerate
accelerate = AccelerateToCatchUp(
self.other_actors[actor_dict.index],
self.ego_vehicles[0],
throttle_value=1,
delta_velocity=actor_dict.delta_speed,
trigger_distance=actor_dict.trigger_distance,
max_distance=100)
sequence.add_child(accelerate)
# lane_change
# lane_change = None
if actor_dict.direction == 'left':
lane_change = LaneChange(
self.other_actors[actor_dict.index],
speed=actor_dict.lane_change_speed,
direction='right',
distance_same_lane=50,
distance_other_lane=10)
sequence.add_child(lane_change)
else:
lane_change = LaneChange(
self.other_actors[actor_dict.index],
speed=actor_dict.lane_change_speed,
direction='left',
distance_same_lane=50,
distance_other_lane=10)
sequence.add_child(lane_change)
sequence.add_child(
WaypointFollower(self.other_actors[actor_dict.index],
target_speed=20,
avoid_collision=True))
endcondition = DriveDistance(
self.other_actors[actor_dict.index], 150)
parallel = py_trees.composites.Sequence(
"Behavior",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel.add_child(sequence)
parallel.add_child(endcondition)
root.add_child(parallel)
elif scenario == "DynamicObstaclesAhead":
lane_width = actor_dict.trigger.lane_width
lane_width = lane_width + (1.25 * lane_width *
actor_dict.num_lane_changes)
start_condition = InTimeToArrivalToVehicle(
self.other_actors[actor_dict.index], self.ego_vehicles[0],
actor_dict.time_to_reach)
# actor_velocity = KeepVelocity(self.other_actors[actor_dict.index],
# actor_dict.speed,
# name="walker velocity")
# actor_drive = DriveDistance(self.other_actors[actor_dict.index],
# 0.5 * lane_width,
# name="walker drive distance")
actor_start_cross_lane = AccelerateToVelocity(
self.other_actors[actor_dict.index],
1.0,
actor_dict.speed,
name="walker crossing lane accelerate velocity")
actor_cross_lane = DriveDistance(
self.other_actors[actor_dict.index],
lane_width,
name="walker drive distance for lane crossing ")
actor_stop_cross_lane = StopVehicle(
self.other_actors[actor_dict.index],
1.0,
name="walker stop")
ego_pass_machine = DriveDistance(
self.ego_vehicles[0], 5, name="ego vehicle passed walker")
# actor_remove = ActorDestroy(self.other_actors[actor_dict.index],
# name="Destroying walker")
scenario_sequence = py_trees.composites.Sequence()
keep_velocity_other = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="keep velocity other")
keep_velocity = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="keep velocity")
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[actor_dict.index],
actor_dict.start_transform,
name='TransformSetterTS3walker',
physics=False))
# scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], True))
scenario_sequence.add_child(start_condition)
# scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], False))
# scenario_sequence.add_child(keep_velocity)
scenario_sequence.add_child(keep_velocity_other)
scenario_sequence.add_child(actor_stop_cross_lane)
# keep_velocity.add_child(actor_velocity)
# keep_velocity.add_child(actor_drive)
keep_velocity_other.add_child(actor_start_cross_lane)
# keep_velocity_other.add_child(actor_cross_lane)
# keep_velocity_other.add_child(ego_pass_machine)
root.add_child(scenario_sequence)
return root
