def _create_behavior(self):
"""
After invoking this scenario, cyclist will wait for the user
controlled vehicle to enter the in the trigger distance region,
the cyclist starts crossing the road once the condition meets,
ego vehicle has to avoid the crash after a right turn, but
continue driving after the road is clear.If this does not happen
within 90 seconds, a timeout stops the scenario.
"""
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="IntersectionRightTurn")
lane_width = self._reference_waypoint.lane_width
lane_width = lane_width + (1.10 * lane_width * self._num_lane_changes)
if self._ego_route is not None:
trigger_distance = InTriggerDistanceToLocationAlongRoute(
self.ego_vehicles[0], self._ego_route,
self._other_actor_transform.location, 20)
else:
trigger_distance = InTriggerDistanceToVehicle(
self.other_actors[0], self.ego_vehicles[0], 20)
actor_velocity = KeepVelocity(self.other_actors[0],
self._other_actor_target_velocity)
actor_traverse = DriveDistance(self.other_actors[0], 0.30 * lane_width)
post_timer_velocity_actor = KeepVelocity(
self.other_actors[0], self._other_actor_target_velocity)
post_timer_traverse_actor = DriveDistance(self.other_actors[0],
0.70 * lane_width)
end_condition = TimeOut(5)
# non leaf nodes
scenario_sequence = py_trees.composites.Sequence()
actor_ego_sync = py_trees.composites.Parallel(
"Synchronization of actor and ego vehicle",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
after_timer_actor = py_trees.composites.Parallel(
"After timout actor will cross the remaining lane_width",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# building the tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform,
name='TransformSetterTS4'))
scenario_sequence.add_child(trigger_distance)
scenario_sequence.add_child(actor_ego_sync)
scenario_sequence.add_child(after_timer_actor)
scenario_sequence.add_child(end_condition)
scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
actor_ego_sync.add_child(actor_velocity)
actor_ego_sync.add_child(actor_traverse)
after_timer_actor.add_child(post_timer_velocity_actor)
after_timer_actor.add_child(post_timer_traverse_actor)
return root
def _create_behavior(self):
"""
Only behavior here is to wait
"""
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)
# leaf nodes
actor_stand = TimeOut(15)
actor_removed = ActorDestroy(self.other_actors[0])
end_condition = DriveDistance(self.ego_vehicles[0],
self._ego_vehicle_distance_driven)
# non leaf nodes
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scenario_sequence = py_trees.composites.Sequence()
# building tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[0], self.transform))
scenario_sequence.add_child(actor_stand)
scenario_sequence.add_child(actor_removed)
scenario_sequence.add_child(end_condition)
return root
def _create_behavior(self):
"""
Scenario behavior:
When close to an intersection, the traffic lights will turn green for
both the ego_vehicle and another lane, allowing the background activity
to "run" their red light, creating scenarios 7, 8 and 9.
If this does not happen within 120 seconds, a timeout stops the scenario
"""
# Changes traffic lights
traffic_hack = TrafficLightManipulator(self.ego_vehicles[0],
self.subtype)
# finally wait that ego vehicle drove a specific distance
wait = DriveDistance(self.ego_vehicles[0],
self._ego_distance_to_drive,
name="DriveDistance")
# Build behavior tree
sequence = py_trees.composites.Sequence("SignalJunctionCrossingRoute")
sequence.add_child(traffic_hack)
sequence.add_child(wait)
return sequence
def _create_behavior(self):
"""
Setup the behavior for NewScenario
"""
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)
# non leaf nodes
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# scenario_sequence = py_trees.composites.Sequence()
num_actors = self._num_actors - 1
while num_actors >= 0:
scenario_sequence = py_trees.composites.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)
root.add_child(scenario_sequence)
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[num_actors],
self.transform))
scenario_sequence.add_child(actor_stand)
scenario_sequence.add_child(actor_removed)
scenario_sequence.add_child(end_condition)
num_actors -= 1
return root
def _create_behavior(self):
sequence = py_trees.composites.Sequence("Sequence Behavior")
parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
parallel.add_child(StopVehicle(self.ego_vehicles[0], 1))
sequence.add_child(parallel)
# match all cars speed
parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
parallel.add_child(KeepVelocity(self.ego_vehicles[0], 2, distance=7))
parallel.add_child(KeepVelocity(self.other_actors[0], 2, distance=7))
sequence.add_child(parallel)
# turn target speed autopilot for all cars
parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
parallel.add_child(
WaypointFollower(self.ego_vehicles[0],
self._speed,
avoid_collision=False))
parallel.add_child(
WaypointFollower(self.other_actors[0],
self._speed,
avoid_collision=False))
sequence.add_child(parallel)
driveDistance = DriveDistance(self.ego_vehicles[0],
self._drivenDistanceM,
name="DriveDistance")
sequence.add_child(driveDistance)
return sequence
def _create_behavior(self):
# endcondition: Check if vehicle reached waypoint _end_distance from here:
end_condition = DriveDistance(self.ego_vehicles[0], self._end_distance)
# Build behavior tree
sequence = py_trees.composites.Sequence("Corl")
sequence.add_child(end_condition)
return sequence
def _create_behavior(self):
"""
The behavior tree returned by this method is as follows:
The ego vehicle is trying to pass a leading vehicle in the same lane
by moving onto the oncoming lane while another vehicle is moving in the
opposite direction in the oncoming lane.
"""
# Leaf nodes
actor_source = ActorSource(
[
"vehicle.audi.tt", "vehicle.tesla.model3",
"vehicle.nissan.micra"
],
self._source_transform,
self._source_gap,
self._blackboard_queue_name,
)
actor_sink = ActorSink(self._sink_location, 10)
ego_drive_distance = DriveDistance(self.ego_vehicles[0],
self._ego_vehicle_drive_distance)
waypoint_follower = WaypointFollower(
self.other_actors[1],
self._opposite_speed,
blackboard_queue_name=self._blackboard_queue_name,
avoid_collision=True,
)
pedestrian_walking = KeepVelocity(self.other_actors[2],
1,
distance=300)
# Non-leaf nodes
parallel_root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# Building tree
parallel_root.add_child(ego_drive_distance)
parallel_root.add_child(actor_source)
parallel_root.add_child(actor_sink)
parallel_root.add_child(waypoint_follower)
parallel_root.add_child(pedestrian_walking)
scenario_sequence = py_trees.composites.Sequence()
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._first_actor_transform))
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[1],
self._second_actor_transform))
# scenario_sequence.add_child(
# ActorTransformSetter(self.other_actors[2],
# self._third_actor_transform))
scenario_sequence.add_child(parallel_root)
scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
scenario_sequence.add_child(ActorDestroy(self.other_actors[1]))
scenario_sequence.add_child(ActorDestroy(self.other_actors[2]))
return scenario_sequence
def _create_behavior(self):
"""
The scenario defined after is a "control loss vehicle" scenario.
"""
sequence = py_trees.composites.Sequence("ControlLoss")
sequence.add_child(
DriveDistance(self.ego_vehicles[0], self._end_distance))
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 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):
"""
Hero vehicle is turning left in an urban area,
at a signalized intersection, while other actor coming straight
.The hero actor may turn left either before other actor
passes intersection or later, without any collision.
After 80 seconds, a timeout stops the scenario.
"""
sequence = py_trees.composites.Sequence("Sequence Behavior")
# Selecting straight path at intersection
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), 0)
# Generating waypoint list till next intersection
plan = []
wp_choice = target_waypoint.next(1.0)
while not wp_choice[0].is_intersection:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(1.0)
# adding flow of actors
actor_source = ActorSource(['vehicle.tesla.model3', 'vehicle.audi.tt'],
self._other_actor_transform, 15,
self._blackboard_queue_name)
# destroying flow of actors
actor_sink = ActorSink(plan[-1][0].transform.location, 10)
# follow waypoints untill next intersection
move_actor = WaypointFollower(
self.other_actors[0],
self._target_vel,
plan=plan,
blackboard_queue_name=self._blackboard_queue_name,
avoid_collision=True)
# wait
wait = DriveDistance(self.ego_vehicles[0], self._ego_distance)
# Behavior tree
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(wait)
root.add_child(actor_source)
root.add_child(actor_sink)
root.add_child(move_actor)
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(root)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
"""
Order of sequence:
- car_visible: spawn car at a visible transform
- just_drive: drive until in trigger distance to ego_vehicle
- accelerate: accelerate to catch up distance to ego_vehicle
- lane_change: change the lane
- endcondition: drive for a defined distance
"""
# car_visible
behaviour = py_trees.composites.Sequence("CarOn_{}_Lane" .format(self._direction))
car_visible = ActorTransformSetter(self.other_actors[0], self._transform_visible)
behaviour.add_child(car_visible)
# just_drive
just_drive = py_trees.composites.Parallel(
"DrivingStraight", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
car_driving = WaypointFollower(self.other_actors[0], self._velocity)
just_drive.add_child(car_driving)
trigger_distance = InTriggerDistanceToVehicle(
self.other_actors[0], self.ego_vehicles[0], self._trigger_distance)
just_drive.add_child(trigger_distance)
behaviour.add_child(just_drive)
# accelerate
accelerate = AccelerateToCatchUp(self.other_actors[0], self.ego_vehicles[0], throttle_value=1,
delta_velocity=self._delta_velocity, trigger_distance=5, max_distance=500)
behaviour.add_child(accelerate)
# lane_change
if self._direction == 'left':
lane_change = LaneChange(
self.other_actors[0], speed=None, direction='right', distance_same_lane=5, distance_other_lane=300)
behaviour.add_child(lane_change)
else:
lane_change = LaneChange(
self.other_actors[0], speed=None, direction='left', distance_same_lane=5, distance_other_lane=300)
behaviour.add_child(lane_change)
# endcondition
endcondition = DriveDistance(self.other_actors[0], 200)
# build tree
root = py_trees.composites.Sequence("Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(behaviour)
root.add_child(endcondition)
return root
def _create_behavior(self):
"""
After invoking this scenario, cyclist will wait for the user
controlled vehicle to enter the in the trigger distance region,
the cyclist starts crossing the road once the condition meets,
ego vehicle has to avoid the crash after a turn, but
continue driving after the road is clear.If this does not happen
within 90 seconds, a timeout stops the scenario.
"""
sequence = py_trees.composites.Sequence()
collision_location = self._collision_wp.transform.location
collision_distance = collision_location.distance(self._adversary_transform.location)
collision_duration = collision_distance / self._adversary_speed
collision_time_trigger = collision_duration + self._reaction_time
# On trigger behavior
if self._ego_route is not None:
sequence.add_child(Scenario4Manager(self._spawn_dist))
# First waiting behavior
sequence.add_child(WaitEndIntersection(self.ego_vehicles[0]))
# Adversary trigger behavior
trigger_adversary = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart")
trigger_adversary.add_child(InTimeToArrivalToLocation(
self.ego_vehicles[0], collision_time_trigger, collision_location))
trigger_adversary.add_child(InTriggerDistanceToLocation(
self.ego_vehicles[0], collision_location, self._min_trigger_dist))
sequence.add_child(trigger_adversary)
sequence.add_child(HandBrakeVehicle(self.other_actors[0], False))
# Move the adversary.
speed_duration = 2.0 * collision_duration
speed_distance = 2.0 * collision_distance
sequence.add_child(KeepVelocity(
self.other_actors[0],
self._adversary_speed,
True,
speed_duration,
speed_distance,
name="AdversaryCrossing")
)
# Remove everything
sequence.add_child(ActorDestroy(self.other_actors[0], name="DestroyAdversary"))
sequence.add_child(DriveDistance(self.ego_vehicles[0], self._ego_end_distance, name="EndCondition"))
return sequence
def _create_behavior(self):
"""
The scenario defined after is a "control loss vehicle" scenario. After
invoking this scenario, it will wait until the vehicle drove a few meters
(_start_distance), and then perform a jitter action. Finally, the vehicle
has to reach a target point (_end_distance). If this does not happen within
60 seconds, a timeout stops the scenario
"""
# start condition
start_end_parallel = py_trees.composites.Parallel("Jitter",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
start_condition = InTriggerDistanceToLocation(self.ego_vehicles[0], self.first_loc_prev, self._trigger_dist)
for _ in range(self._no_of_jitter):
# change the current noise to be applied
turn = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise,
self._noise_mean, self._noise_std, self._dynamic_mean_for_steer,
self._dynamic_mean_for_throttle) # Mean value of steering noise
# Noise end! put again the added noise to zero.
noise_end = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise,
0, 0, 0, 0)
jitter_action = py_trees.composites.Parallel("Jitter",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# Abort jitter_sequence, if the vehicle is approaching an intersection
jitter_abort = InTriggerDistanceToNextIntersection(self.ego_vehicles[0], self._abort_distance_to_intersection)
# endcondition: Check if vehicle reached waypoint _end_distance from here:
end_condition = DriveDistance(self.ego_vehicles[0], self._end_distance)
start_end_parallel.add_child(start_condition)
start_end_parallel.add_child(end_condition)
# Build behavior tree
sequence = py_trees.composites.Sequence("ControlLoss")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self.first_transform, physics=False))
sequence.add_child(ActorTransformSetter(self.other_actors[1], self.sec_transform, physics=False))
sequence.add_child(ActorTransformSetter(self.other_actors[2], self.third_transform, physics=False))
jitter = py_trees.composites.Sequence("Jitter Behavior")
jitter.add_child(turn)
jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.sec_loc_prev, self._trigger_dist))
jitter.add_child(turn)
jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.third_loc_prev, self._trigger_dist))
jitter.add_child(turn)
jitter_action.add_child(jitter)
jitter_action.add_child(jitter_abort)
sequence.add_child(start_end_parallel)
sequence.add_child(jitter_action)
sequence.add_child(end_condition)
sequence.add_child(noise_end)
return sequence
def _create_behavior(self):
"""
Only behavior here is to wait
"""
# leaf nodes
actor_stand = Idle(15)
end_condition = DriveDistance(
self.ego_vehicles[0],
self._ego_vehicle_distance_driven)
# non leaf nodes
scenario_sequence = py_trees.composites.Sequence()
# building tree
scenario_sequence.add_child(actor_stand)
for i, _ in enumerate(self.other_actors):
scenario_sequence.add_child(ActorDestroy(self.other_actors[i]))
scenario_sequence.add_child(end_condition)
return scenario_sequence
def _create_behavior(self):
"""
Scenario behavior:
When close to an intersection, the traffic lights will turn green for
both the ego_vehicle and another lane, allowing the background activity
to "run" their red light.
If this does not happen within 120 seconds, a timeout stops the scenario
"""
# finally wait that ego vehicle drove a specific distance
wait = WaitEndIntersection(self.ego_vehicles[0],
name="WaitEndIntersection")
end_condition = DriveDistance(self.ego_vehicles[0],
self._ego_distance_to_drive,
name="DriveDistance")
# Build behavior tree
sequence = py_trees.composites.Sequence(
"NoSignalJunctionCrossingRoute")
sequence.add_child(wait)
sequence.add_child(end_condition)
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
"""
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
def _create_behavior(self):
"""
Scenario behavior:
The other vehicle waits until the ego vehicle is close enough to the
intersection and that its own traffic light is red. Then, it will start
driving and 'illegally' cross the intersection. After a short distance
it should stop again, outside of the intersection. The ego vehicle has
to avoid the crash, but continue driving after the intersection is clear.
If this does not happen within 120 seconds, a timeout stops the scenario
"""
crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0])
# start condition
startcondition = InTriggerDistanceToLocation(
self.ego_vehicles[0],
crossing_point_dynamic,
self._ego_distance_to_traffic_light,
name="Waiting for start position")
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
location_of_collision_dynamic = get_geometric_linear_intersection(
self.ego_vehicles[0], self.other_actors[0])
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
location_of_collision_dynamic)
sync_arrival_stop = InTriggerDistanceToNextIntersection(
self.other_actors[0], 5)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
# Generate plan for WaypointFollower
turn = 0 # drive straight ahead
plan = []
# generating waypoints until intersection (target_waypoint)
plan, target_waypoint = generate_target_waypoint_list(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), turn)
# Generating waypoint list till next intersection
wp_choice = target_waypoint.next(5.0)
while len(wp_choice) == 1:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(5.0)
continue_driving = py_trees.composites.Parallel(
"ContinueDriving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
continue_driving_waypoints = WaypointFollower(
self.other_actors[0],
self._other_actor_target_velocity,
plan=plan,
avoid_collision=False)
continue_driving_distance = DriveDistance(self.other_actors[0],
self._other_actor_distance,
name="Distance")
continue_driving_timeout = TimeOut(10)
continue_driving.add_child(continue_driving_waypoints)
continue_driving.add_child(continue_driving_distance)
continue_driving.add_child(continue_driving_timeout)
# finally wait that ego vehicle drove a specific distance
wait = DriveDistance(self.ego_vehicles[0],
self._ego_distance_to_drive,
name="DriveDistance")
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(startcondition)
sequence.add_child(sync_arrival_parallel)
sequence.add_child(continue_driving)
sequence.add_child(wait)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
"""
The scenario defined after is a "other leading vehicle" scenario. After
invoking this scenario, the user controlled vehicle has to drive towards the
moving other actors, then make the leading actor to decelerate when user controlled
vehicle is at some close distance. Finally, the user-controlled vehicle has to change
lane to avoid collision and follow other leading actor in other lane to end the scenario.
If this does not happen within 90 seconds, a timeout stops the scenario or the ego vehicle
drives certain distance and stops the scenario.
"""
# start condition
driving_in_same_direction = py_trees.composites.Parallel(
"All actors driving in same direction",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
leading_actor_sequence_behavior = py_trees.composites.Sequence(
"Decelerating actor sequence behavior")
# both actors moving in same direction
keep_velocity = py_trees.composites.Parallel(
"Trigger condition for deceleration",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity.add_child(
WaypointFollower(self.other_actors[0],
self._first_vehicle_speed,
avoid_collision=True))
keep_velocity.add_child(
InTriggerDistanceToVehicle(self.other_actors[0],
self.ego_vehicles[0], 55))
# Decelerating actor sequence behavior
decelerate = self._first_vehicle_speed / 3.2
leading_actor_sequence_behavior.add_child(keep_velocity)
leading_actor_sequence_behavior.add_child(
WaypointFollower(self.other_actors[0],
decelerate,
avoid_collision=True))
# end condition
ego_drive_distance = DriveDistance(self.ego_vehicles[0],
self._ego_vehicle_drive_distance)
# Build behavior tree
sequence = py_trees.composites.Sequence("Scenario behavior")
parallel_root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_root.add_child(ego_drive_distance)
parallel_root.add_child(driving_in_same_direction)
driving_in_same_direction.add_child(leading_actor_sequence_behavior)
driving_in_same_direction.add_child(
WaypointFollower(self.other_actors[1],
self._second_vehicle_speed,
avoid_collision=True))
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(parallel_root)
sequence.add_child(ActorDestroy(self.other_actors[0]))
sequence.add_child(ActorDestroy(self.other_actors[1]))
return sequence
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 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 _create_behavior(self):
"""
After invoking this scenario, cyclist will wait for the user
controlled vehicle to enter trigger distance region,
the cyclist starts crossing the road once the condition meets,
then after 60 seconds, a timeout stops the scenario
"""
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE,
name="OccludedObjectCrossing")
lane_width = self._reference_waypoint.lane_width
lane_width = lane_width + (1.25 * lane_width * self._num_lane_changes)
dist_to_trigger = 12 + self._num_lane_changes
# leaf nodes
if self._ego_route is not None:
start_condition = InTriggerDistanceToLocationAlongRoute(
self.ego_vehicles[0], self._ego_route, self.transform.location,
dist_to_trigger)
else:
start_condition = InTimeToArrivalToVehicle(self.other_actors[0],
self.ego_vehicles[0],
self._time_to_reach)
actor_velocity = KeepVelocity(self.other_actors[0],
self._other_actor_target_velocity,
name="walker velocity")
actor_drive = DriveDistance(self.other_actors[0],
0.5 * lane_width,
name="walker drive distance")
actor_start_cross_lane = AccelerateToVelocity(
self.other_actors[0],
1.0,
self._other_actor_target_velocity,
name="walker crossing lane accelerate velocity")
actor_cross_lane = DriveDistance(
self.other_actors[0],
lane_width,
name="walker drive distance for lane crossing ")
actor_stop_crossed_lane = StopVehicle(self.other_actors[0],
self._other_actor_max_brake,
name="walker stop")
ego_pass_machine = DriveDistance(self.ego_vehicles[0],
5,
name="ego vehicle passed prop")
actor_remove = ActorDestroy(self.other_actors[0],
name="Destroying walker")
static_remove = ActorDestroy(self.other_actors[1],
name="Destroying Prop")
end_condition = DriveDistance(self.ego_vehicles[0],
self._ego_vehicle_distance_driven,
name="End condition ego drive distance")
# non leaf nodes
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")
# building tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self.transform,
name='TransformSetterTS3walker',
physics=False))
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[1],
self.transform2,
name='TransformSetterTS3coca',
physics=False))
scenario_sequence.add_child(
HandBrakeVehicle(self.other_actors[0], True))
scenario_sequence.add_child(start_condition)
scenario_sequence.add_child(
HandBrakeVehicle(self.other_actors[0], False))
scenario_sequence.add_child(keep_velocity)
scenario_sequence.add_child(keep_velocity_other)
scenario_sequence.add_child(actor_stop_crossed_lane)
scenario_sequence.add_child(actor_remove)
scenario_sequence.add_child(static_remove)
scenario_sequence.add_child(end_condition)
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)
return root
def _create_behavior(self):
"""
Hero vehicle is turning right in an urban area,
at a signalized intersection, while other actor coming straight
from left.The hero actor may turn right either before other actor
passes intersection or later, without any collision.
After 80 seconds, a timeout stops the scenario.
"""
location_of_collision_dynamic = get_geometric_linear_intersection(
self.ego_vehicles[0], self.other_actors[0])
crossing_point_dynamic = get_crossing_point(self.other_actors[0])
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
location_of_collision_dynamic)
sync_arrival_stop = InTriggerDistanceToLocation(
self.other_actors[0], crossing_point_dynamic, 5)
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
# Selecting straight path at intersection
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), 0)
# Generating waypoint list till next intersection
plan = []
wp_choice = target_waypoint.next(1.0)
while not wp_choice[0].is_intersection:
target_waypoint = wp_choice[0]
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(1.0)
move_actor = WaypointFollower(self.other_actors[0],
self._target_vel,
plan=plan)
waypoint_follower_end = InTriggerDistanceToLocation(
self.other_actors[0], plan[-1][0].transform.location, 10)
move_actor_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
move_actor_parallel.add_child(move_actor)
move_actor_parallel.add_child(waypoint_follower_end)
#manipulate traffic light
manipulate_traffic_light = TrafficLightManipulator(
self.ego_vehicles[0], "S7right")
move_actor_parallel.add_child(manipulate_traffic_light)
# stop other actor
stop = StopVehicle(self.other_actors[0], self._brake_value)
# end condition
end_condition = DriveDistance(self.ego_vehicles[0], self._ego_distance)
# Behavior tree
sequence = py_trees.composites.Sequence()
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(sync_arrival_parallel)
sequence.add_child(move_actor_parallel)
sequence.add_child(stop)
sequence.add_child(end_condition)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
"""
"""
# building the tree
scenario_sequence = py_trees.composites.Sequence()
waypoint_events = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
destroy_actors = py_trees.composites.Sequence()
reach_destination = InTriggerDistanceToLocation(
self.ego_vehicles[0], self.customized_data['destination'], 2)
scenario_sequence.add_child(waypoint_events)
scenario_sequence.add_child(reach_destination)
scenario_sequence.add_child(destroy_actors)
for i in range(len(self.pedestrian_list)):
pedestrian_actor, pedestrian_generated_transform = self.pedestrian_list[
i]
pedestrian_info = self.customized_data['pedestrian_list'][i]
trigger_distance = InTriggerDistanceToVehicle(
self.ego_vehicles[0], pedestrian_actor,
pedestrian_info.trigger_distance)
movement = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
actor_velocity = KeepVelocity(pedestrian_actor,
pedestrian_info.speed)
actor_traverse = DriveDistance(pedestrian_actor,
pedestrian_info.dist_to_travel)
movement.add_child(actor_velocity)
movement.add_child(actor_traverse)
if pedestrian_info.after_trigger_behavior == 'destroy':
after_trigger_behavior = ActorDestroy(pedestrian_actor)
elif pedestrian_info.after_trigger_behavior == 'stop':
after_trigger_behavior = StopVehicle(pedestrian_actor,
brake_value=0.5)
destroy_actor = ActorDestroy(pedestrian_actor)
destroy_actors.add_child(destroy_actor)
else:
raise
pedestrian_behaviors = py_trees.composites.Sequence()
pedestrian_behaviors.add_child(trigger_distance)
pedestrian_behaviors.add_child(movement)
pedestrian_behaviors.add_child(after_trigger_behavior)
waypoint_events.add_child(pedestrian_behaviors)
for i in range(len(self.vehicle_list)):
vehicle_actor, generated_transform = self.vehicle_list[i]
vehicle_info = self.customized_data['vehicle_list'][i]
keep_velocity = py_trees.composites.Parallel(
"Trigger condition for changing behavior",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity.add_child(
InTriggerDistanceToVehicle(self.ego_vehicles[0], vehicle_actor,
vehicle_info.trigger_distance))
keep_velocity.add_child(
WaypointFollower(vehicle_actor,
vehicle_info.initial_speed,
avoid_collision=vehicle_info.avoid_collision))
if vehicle_info.waypoint_follower:
# interpolate current location and destination to find a path
start_location = generated_transform.location
end_location = vehicle_info.targeted_waypoint.location
_, route = interpolate_trajectory(
self.world, [start_location, end_location])
ds_ids = downsample_route(
route, self.customized_data['sample_factor'])
route = [(route[x][0], route[x][1]) for x in ds_ids]
# print('route', len(route))
perturb_route(route, vehicle_info.waypoints_perturbation)
# visualize_route(route)
plan = []
for transform, cmd in route:
wp = self._wmap.get_waypoint(transform.location,
project_to_road=False,
lane_type=carla.LaneType.Any)
if not wp:
wp = self._wmap.get_waypoint(
transform.location,
project_to_road=True,
lane_type=carla.LaneType.Any)
print('(', transform.location.x, transform.location.y,
')', 'is replaced by', '(',
wp.transform.location.x, wp.transform.location.y,
')')
plan.append((wp, cmd))
movement = WaypointFollower(
actor=vehicle_actor,
target_speed=vehicle_info.targeted_speed,
plan=plan,
avoid_collision=vehicle_info.avoid_collision)
else:
movement = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
actor_velocity = KeepVelocity(
vehicle_actor,
vehicle_info.targeted_speed,
target_direction=vehicle_info.target_direction)
actor_traverse = DriveDistance(vehicle_actor,
vehicle_info.dist_to_travel)
movement.add_child(actor_velocity)
movement.add_child(actor_traverse)
if vehicle_info.after_trigger_behavior == 'destroy':
after_trigger_behavior = ActorDestroy(vehicle_actor)
elif vehicle_info.after_trigger_behavior == 'stop':
after_trigger_behavior = StopVehicle(vehicle_actor,
brake_value=0.5)
destroy_actor = ActorDestroy(vehicle_actor)
destroy_actors.add_child(destroy_actor)
else:
raise
vehicle_behaviors = py_trees.composites.Sequence()
vehicle_behaviors.add_child(keep_velocity)
vehicle_behaviors.add_child(movement)
vehicle_behaviors.add_child(after_trigger_behavior)
waypoint_events.add_child(vehicle_behaviors)
return scenario_sequence