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.
"""
# leaf nodes
trigger_distance = InTriggerDistanceToVehicle(
self.other_actors[0],
self.ego_vehicle,
self._trigger_distance_from_ego)
stop_other_actor = StopVehicle(
self.other_actors[0],
self._other_actor_max_brake)
timeout_other_actor = TimeOut(5)
start_other = KeepVelocity(
self.other_actors[0],
self._other_actor_target_velocity)
trigger_other = InTriggerRegion(
self.other_actors[0],
85.5, 86.5,
41, 43)
stop_other = StopVehicle(
self.other_actors[0],
self._other_actor_max_brake)
timeout_other = TimeOut(3)
sync_arrival = SyncArrival(
self.other_actors[0], self.ego_vehicle, self._location_of_collision)
sync_arrival_stop = InTriggerDistanceToVehicle(self.other_actors[0],
self.ego_vehicle,
6)
# non leaf nodes
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scenario_sequence = py_trees.composites.Sequence()
keep_velocity_other = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
sync_arrival_parallel = py_trees.composites.Parallel(
"Synchronize arrival times",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# building the tress
root.add_child(scenario_sequence)
scenario_sequence.add_child(trigger_distance)
scenario_sequence.add_child(sync_arrival_parallel)
scenario_sequence.add_child(stop_other_actor)
scenario_sequence.add_child(timeout_other_actor)
scenario_sequence.add_child(keep_velocity_other)
scenario_sequence.add_child(stop_other)
scenario_sequence.add_child(timeout_other)
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(sync_arrival_stop)
keep_velocity_other.add_child(start_other)
keep_velocity_other.add_child(trigger_other)
return root
def _create_behavior(self):
"""
The ego vehicle is initially at rest behind a tall lead vehicle at a
traffic light. The lead vehicle waits for the light to change and
proceeds. The ego vehicle either proceeds immediately (variant a)
or waits until the light is visible (variant b).
"""
other_drive = py_trees.composites.Sequence("Other Sequence")
other_drive.add_child(KeepVelocity(self.other_actors[0], target_velocity=self._other_actor_target_speed, distance=80))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
if (self._variant == "Rss_Ext_TEMPOCCL_b"):
ego_drive.add_child(TimeOut(3))
ego_drive.add_child(RssExtBehavior(self._rss_params, self.ego_vehicles[0], self._ego_target_speed, self._target, variant=self._variant))
parallel_drive = py_trees.composites.Parallel("All Cars Driving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(other_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_0_transform))
sequence.add_child(TimeOut(13))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
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
"""
# let the other actor drive until next intersection
# @todo: We should add some feedback mechanism to respond to ego_vehicle behavior
other_driving_to_next_intersection = py_trees.composites.Parallel("DrivingTowardsIntersection", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
other_driving_to_next_intersection.add_child(WaypointFollower(self.other_actors[0], self._other_actor_target_speed))
other_driving_to_next_intersection.add_child(InTriggerDistanceToNextIntersection(
self.other_actors[0], self._other_actor_stop_in_front_intersection))
other_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=10,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0], name="StandStill")
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
ego_drives = py_trees.composites.Parallel("ego", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_drives.add_child(RssBasicAgentBehavior(self._rss_params, self.ego_vehicles[0], self._ego_target_speed, self._target))
ego_drives.add_child(endcondition)
other_drives = py_trees.composites.Sequence("Sequence Behavior")
other_drives.add_child(other_driving_to_next_intersection)
other_drives.add_child(other_stop)
other_drives.add_child(TimeOut(float('inf')))
parallel_drive = py_trees.composites.Parallel("DrivingTowardsIntersection", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(other_drives)
parallel_drive.add_child(ego_drives)
# 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(TimeOut(3))
sequence.add_child(parallel_drive)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
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,
then after 60 seconds, a timeout stops the scenario
"""
# leaf nodes
trigger_dist = InTriggerDistanceToVehicle(
self.other_actors[0], self.ego_vehicle,
self._trigger_distance_from_ego)
start_other_actor = KeepVelocity(self.other_actors[0],
self._other_actor_target_velocity)
trigger_other = InTriggerRegion(self.other_actors[0], 46, 50, 128,
129.5)
stop_other_actor = StopVehicle(self.other_actors[0],
self._other_actor_max_brake)
timeout_other = TimeOut(10)
start_vehicle = KeepVelocity(self.other_actors[0],
self._other_actor_target_velocity)
trigger_other_actor = InTriggerRegion(self.other_actors[0], 46, 50,
137, 139)
stop_vehicle = StopVehicle(self.other_actors[0],
self._other_actor_max_brake)
timeout_other_actor = TimeOut(3)
# non leaf nodes
root = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scenario_sequence = py_trees.composites.Sequence()
keep_velocity_other_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity_other = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# building tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(trigger_dist)
scenario_sequence.add_child(keep_velocity_other_parallel)
scenario_sequence.add_child(stop_other_actor)
scenario_sequence.add_child(timeout_other)
scenario_sequence.add_child(keep_velocity_other)
scenario_sequence.add_child(stop_vehicle)
scenario_sequence.add_child(timeout_other_actor)
keep_velocity_other_parallel.add_child(start_other_actor)
keep_velocity_other_parallel.add_child(trigger_other)
keep_velocity_other.add_child(start_vehicle)
keep_velocity_other.add_child(trigger_other_actor)
return root
def _create_behavior(self):
"""
Parking lot behavior
The ego vehicle drives out of a parking lot towards a destination
outside of the parking lot. The scenario ends after the ego vehicle
has driven 50 meters.
"""
ego_drive = py_trees.composites.Parallel(
"Ego Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
ego_drive.add_child(DriveDistance(self.ego_vehicles[0], 50))
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(TimeOut(1))
sequence.add_child(ego_drive)
return sequence
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 __init__(self, behavior, criteria, name, timeout=60, terminate_on_failure=False):
self.behavior = behavior
self.test_criteria = criteria
self.timeout = timeout
self.name = name
if self.test_criteria is not None and not isinstance(self.test_criteria, py_trees.composites.Parallel):
# list of nodes
for criterion in self.test_criteria:
criterion.terminate_on_failure = terminate_on_failure
# Create py_tree for test criteria
self.criteria_tree = py_trees.composites.Parallel(name="Test Criteria")
self.criteria_tree.add_children(self.test_criteria)
self.criteria_tree.setup(timeout=1)
else:
self.criteria_tree = criteria
# Create node for timeout
self.timeout_node = TimeOut(self.timeout, name="TimeOut")
# Create overall py_tree
self.scenario_tree = py_trees.composites.Parallel(name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
if behavior is not None:
self.scenario_tree.add_child(self.behavior)
self.scenario_tree.add_child(self.timeout_node)
if criteria is not None:
self.scenario_tree.add_child(self.criteria_tree)
self.scenario_tree.setup(timeout=1)
def _create_behavior(self):
"""
Passing a pedestrian behavior.
The ego vehicle drives a distance of 50 meters, or stops for the
pedestrian, depending on the scenario variant.
"""
ego_drive = py_trees.composites.Parallel(
"Ego Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
if (self._variant == 'Rss_Ext_PASSPED_b'):
sub_drive = py_trees.composites.Sequence("Ego Stopping")
sub_drive.add_child(DriveDistance(self.ego_vehicles[0], 20))
sub_drive.add_child(
StopVehicle(self.ego_vehicles[0],
self._rss_params['alpha_lon_brake_min']))
ego_drive.add_child(sub_drive)
else:
ego_drive.add_child(DriveDistance(self.ego_vehicles[0], 50))
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(TimeOut(1))
sequence.add_child(ego_drive)
sequence.add_child(ActorDestroy(self._pedestrian))
return sequence
def _create_behavior(self):
"""
Side Incursion behavior
The lead vehicle remains still for 10 seconds and is partially obstructing the ego vehicle's lane.
The ego vehicle approaches the lead vehicle from behind.
The scenario ends after ten seconds.
"""
lead_drive = py_trees.composites.Sequence("Lead Sequence")
lead_drive.add_child(Idle(duration=10))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(RssExtBehavior(self._rss_params, self.ego_vehicles[0], self._ego_target_speed, self._target, variant=self._variant))
parallel_drive = py_trees.composites.Parallel("All Cars Driving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(lead_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self._lead_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
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):
# EGO DRIVES
ego_turn = 0 # drives straight
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.ego_vehicles[0].get_location()), ego_turn)
wp_choice = target_waypoint.next(30.0)
target_waypoint = wp_choice[0]
destination_location = target_waypoint.transform.location
print(destination_location)
ego_driving = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_driving.add_child(
RssBasicAgentBehavior(self._rss_params, self.ego_vehicles[0],
self._ego_target_speed,
destination_location))
ego_driving.add_child(
InTriggerDistanceToLocation(self.ego_vehicles[0],
destination_location, 15))
# POV DRIVES
plan = []
turn = -1 # turn left
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)
pov_driving = py_trees.composites.Sequence("Sequence Behavior")
pov_driving_intersection = py_trees.composites.Parallel(
"ContinueDriving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
pov_driving_intersection.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed,
plan=plan,
avoid_collision=False))
pov_driving_intersection.add_child(
DriveDistance(self.other_actors[0], 40, name="Distance"))
pov_driving.add_child(pov_driving_intersection)
pov_driving.add_child(TimeOut(float('inf')))
########################
parallel_drive = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(pov_driving)
parallel_drive.add_child(ego_driving)
# FINAL SEQUENCE
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(parallel_drive)
#sequence.add_child(TimeOut(100))
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 __init__(self,
behavior,
criteria,
name,
timeout=60,
terminate_on_failure=False):
self.behavior = behavior
self.test_criteria = criteria
self.timeout = timeout
self.name = name
if self.test_criteria is not None and not isinstance(
self.test_criteria, py_trees.composites.Parallel):
# list of nodes
for criterion in self.test_criteria:
try:
flag = criterion._terminate_on_failure
except:
print('Such attribute doesn\'t exist.')
criterion._terminate_on_failure = terminate_on_failure
# Create py_tree for test criteria
self.criteria_tree = py_trees.composites.Parallel(
name="Test Criteria",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
self.criteria_tree.add_children(self.test_criteria)
self.criteria_tree.setup(timeout=1)
else:
self.criteria_tree = criteria
# Create node for timeout
self.timeout_node = TimeOut(self.timeout, name="TimeOut")
# Create overall py_tree
self.scenario_tree = py_trees.composites.Parallel(
name, policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
if behavior is not None:
self.scenario_tree.add_child(self.behavior)
self.scenario_tree.add_child(self.timeout_node)
self.scenario_tree.add_child(WeatherBehavior())
self.scenario_tree.add_child(UpdateAllActorControls())
if criteria is not None:
self.scenario_tree.add_child(self.criteria_tree)
self.scenario_tree.setup(timeout=1)
def _create_behavior(self):
"""
Occlusion behavior.
A vehicle is traveling straight through a T intersection.
A tree partially occludes view of the T intersection.
The ego vehicle approaches the intersection from a right angle and must
plan how to yield to the traveling vehicle.
"""
other_0_drive = py_trees.composites.Sequence("Other Sequence")
other_0_drive.add_child(
KeepVelocity(self.other_actors[0],
target_velocity=self._other_actor_target_speed,
distance=90))
#lead_drive.add_child(DriveDistance(self.other_actors[0], 30))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
parallel_drive = py_trees.composites.Parallel(
"All Cars Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(other_0_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_0_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
return sequence
def _create_behavior(self):
"""
Merge behavior.
A fleet of four vehicles is traveling along a lane with a gap between
vehicles 2 and 3. The vehicles travel for 110 meters.
The ego vehicle attempts to merge into the gap.
"""
other_0_drive = py_trees.composites.Sequence("Other 0 Sequence")
other_0_drive.add_child(KeepVelocity(self.other_actors[0], target_velocity=self._other_actor_target_speed, distance=110))
other_1_drive = py_trees.composites.Sequence("Other 1 Sequence")
other_1_drive.add_child(KeepVelocity(self.other_actors[1], target_velocity=self._other_actor_target_speed, distance=110))
other_2_drive = py_trees.composites.Sequence("Other 2 Sequence")
other_2_drive.add_child(KeepVelocity(self.other_actors[2], target_velocity=self._other_actor_target_speed, distance=110))
other_3_drive = py_trees.composites.Sequence("Other 3 Sequence")
other_3_drive.add_child(KeepVelocity(self.other_actors[3], target_velocity=self._other_actor_target_speed, distance=110))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(RssExtBehavior(self._rss_params, self.ego_vehicles[0], self._ego_target_speed, self._target, variant=self._variant))
parallel_drive = py_trees.composites.Parallel("All Cars Driving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(other_0_drive)
parallel_drive.add_child(other_1_drive)
parallel_drive.add_child(other_2_drive)
parallel_drive.add_child(other_3_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self._other_0_transform))
sequence.add_child(ActorTransformSetter(self.other_actors[1], self._other_1_transform))
sequence.add_child(ActorTransformSetter(self.other_actors[2], self._other_2_transform))
sequence.add_child(ActorTransformSetter(self.other_actors[3], self._other_3_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
return sequence
def _create_behavior(self):
"""
Shorter following distance multiple geometry behavior.
The lead vehicle drives for 60 meters, passing through a roundabout.
The ego vehicle follows the lead vehicle for a portion of the roundabout.
"""
other_drive = py_trees.composites.Parallel(
"Other Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
other_drive.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed))
other_drive.add_child(DriveDistance(self.other_actors[0], 60))
ego_drive = py_trees.composites.Sequence("Ego Driving")
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
parallel_drive = py_trees.composites.Parallel(
"All Cars Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(other_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(
ActorTransformSetter(self.other_actors[0], self._other_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
return sequence
def _create_behavior(self):
"""
Shorter Following Distance behavior
The lead vehicle drives for 100 meters.
The ego vehicle follows at the RSS Classic or RSS Extended safe
following distance.
"""
lead_drive = py_trees.composites.Sequence("Lead Sequence")
lead_drive.add_child(
KeepVelocity(self.other_actors[0],
target_velocity=self._other_actor_target_speed,
distance=100))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
parallel_drive = py_trees.composites.Parallel(
"All Cars Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(lead_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(
ActorTransformSetter(self.other_actors[0], self._lead_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
return sequence
def _create_behavior(self):
"""
Forward Incursion behavior
All vehicles are initially at rest.
- Lead vehicle accelerates to target speed, driving for 60 meters total.
- Cut-in vehicle changes lanes at 10 meters, driving for 60 meters total.
- Ego is intially following lead vehicle and brakes to RSS safe following distance after cut-in.
"""
lead_drive = py_trees.composites.Sequence("Lead Sequence")
lead_drive.add_child(KeepVelocity(self.other_actors[0], target_velocity=self._other_actor_target_speed, distance=60))
cut_in_drive = py_trees.composites.Sequence("Cut-In Sequence")
cut_in_drive.add_child(LaneChange(self.other_actors[1], speed=self._cut_in_target_speed, direction="right", distance_same_lane=10, distance_other_lane=50))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(RssExtBehavior(self._rss_params, self.ego_vehicles[0], self._ego_target_speed, self._target, variant=self._variant))
two_other_cars_drive = py_trees.composites.Parallel("Other Cars Driving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
two_other_cars_drive.add_child(lead_drive)
two_other_cars_drive.add_child(cut_in_drive)
parallel_drive = py_trees.composites.Parallel("All Cars Driving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(two_other_cars_drive)
parallel_drive.add_child(ego_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(ActorTransformSetter(self.other_actors[0], self._lead_transform))
sequence.add_child(ActorTransformSetter(self.other_actors[1], self._lane_changer_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
return sequence
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 convert_condition_to_atomic(condition, actor_list):
"""
Convert an OpenScenario condition into a Behavior/Criterion atomic
Note: Not all conditions are currently supported.
"""
atomic = None
condition_name = condition.attrib.get('name')
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')
if ttc_condition.attrib.get('rule') != "less_than":
raise NotImplementedError(
"TimeToCollision condition with the given specification is not yet supported"
)
condition_value = ttc_condition.find('value')
condition_target = ttc_condition.find('Target')
if condition_target.find('Position'):
position = condition_target.find('Position')
transform = OpenScenarioParser.convert_position_to_transform(
position)
atomic = InTimeToArrivalToLocation(
triggered_actor, condition_value,
condition_target.location)
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)
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')
transform = OpenScenarioParser.convert_position_to_transform(
position)
atomic = InTriggerDistanceToLocation(trigger_actor,
transform.location,
distance_value,
name=condition_name)
elif entity_condition.find('Distance') is not None:
distance_condition = entity_condition.find(
'RelativeDistance')
distance_value = float(
distance_condition.attrib.get('value'))
if distance_condition.attrib.get('rule') != "less_than":
raise NotImplementedError(
"Distance condition with the given specification is not yet supported"
)
if distance_condition.find('Position'):
position = distance_condition.find('Position')
transform = OpenScenarioParser.convert_position_to_transform(
position)
atomic = InTriggerDistanceToLocation(
triggered_actor,
transform.location,
distance_value,
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('rule') == "less_than"
and 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)))
atomic = InTriggerDistanceToVehicle(
triggered_actor,
trigger_actor,
distance_value,
name=condition_name)
else:
raise NotImplementedError(
"RelativeDistance condition with the given specification is not yet supported"
)
elif condition.find('ByState') is not None:
raise NotImplementedError(
"ByState conditions are not yet supported")
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 = simtime_condition.attrib.get('value')
rule = simtime_condition.attrib.get('value')
if rule != "greater_than":
raise NotImplementedError(
"ByValue SimulationTime conditions with the given specification is not yet supported"
)
atomic = TimeOut(value)
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")
return 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
"""
# start condition
startcondition = InTimeToArrivalToLocation(self.ego_vehicle,
self._ego_other_distance_start,
self.other_actors[0].get_location(),
name="Waiting for start position")
# 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(
"Waiting for end position",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
driving_considering_bike = py_trees.composites.Parallel(
"Drive with AutoPilot",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
driving_considering_bike.add_child(UseAutoPilot(self.other_actors[0]))
obstacle_sequence = py_trees.composites.Sequence("Obstacle sequence behavior")
obstacle_sequence.add_child(InTriggerDistanceToVehicle(self.other_actors[0],
self.other_actors[1],
10))
obstacle_sequence.add_child(TimeOut(5))
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], 5))
obstacle_sequence.add_child(obstacle_clear_road)
obstacle_sequence.add_child(StopVehicle(self.other_actors[1], self._other_actor_max_brake))
driving_considering_bike.add_child(obstacle_sequence)
driving_to_next_intersection.add_child(InTriggerDistanceToNextIntersection(
self.other_actors[0], self._other_actor_stop_in_front_intersection))
driving_to_next_intersection.add_child(driving_considering_bike)
# 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_vehicle,
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicle, 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(startcondition)
sequence.add_child(driving_to_next_intersection)
sequence.add_child(stop)
sequence.add_child(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
location, _ = get_location_in_distance(self.ego_vehicle,
self._start_distance)
start_condition = InTriggerDistanceToLocation(self.ego_vehicle,
location, 2.0)
# jitter sequence
jitter_sequence = py_trees.composites.Sequence(
"Jitter Sequence Behavior")
jitter_timeout = TimeOut(timeout=0.2, name="Timeout for next jitter")
for i in range(self._no_of_jitter_actions):
ego_vehicle_max_steer = random.gauss(self._noise_mean,
self._noise_std)
if ego_vehicle_max_steer > 0:
ego_vehicle_max_steer += self._dynamic_mean
elif ego_vehicle_max_steer < 0:
ego_vehicle_max_steer -= self._dynamic_mean
# turn vehicle
turn = SteerVehicle(self.ego_vehicle,
ego_vehicle_max_steer,
name='Steering ' + str(i))
jitter_action = py_trees.composites.Parallel(
"Jitter Actions with Timeouts",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
jitter_action.add_child(turn)
jitter_action.add_child(jitter_timeout)
jitter_sequence.add_child(jitter_action)
# Abort jitter_sequence, if the vehicle is approaching an intersection
jitter_abort = InTriggerDistanceToNextIntersection(
self.ego_vehicle, self._abort_distance_to_intersection)
jitter = py_trees.composites.Parallel(
"Jitter", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
jitter.add_child(jitter_sequence)
jitter.add_child(jitter_abort)
# endcondition: Check if vehicle reached waypoint _end_distance from here:
location, _ = get_location_in_distance(self.ego_vehicle,
self._end_distance)
end_condition = InTriggerDistanceToLocation(self.ego_vehicle, location,
2.0)
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(start_condition)
sequence.add_child(jitter)
sequence.add_child(end_condition)
return sequence
def _create_behavior(self):
acceleration_value = 0.7 # how to turn into g?
braking_value_soft = 0.1 #
braking_value_hard = 0.6 #
wait_time = 6 #sec
dist_endcond = 60 # depends on the behavior of the front car. change if the acc/braking vals are different
##############################################
#------------------------------------
p1 = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
p1.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed))
p1.add_child(
TriggerVelocity(self.other_actors[0],
self._other_actor_target_speed))
#------------------------------------
p2 = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
p2.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed))
p2.add_child(TimeOut(wait_time))
#------------------------------------
p3 = StopVehicle(self.other_actors[0], braking_value_soft)
#-------------------------
p4 = TimeOut(wait_time)
#------------------------------------
p5 = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
p5.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed))
p5.add_child(
AccelerateToVelocity(self.other_actors[0], acceleration_value,
self._other_actor_target_speed))
#------------------------------------
p6 = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
p6.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_speed))
p6.add_child(TimeOut(wait_time))
#------------------------------------
p7 = StopVehicle(self.other_actors[0], braking_value_hard)
#------------------------------------
p8 = TimeOut(float('inf'))
#------------------------------------
pov_driving = py_trees.composites.Sequence()
pov_driving.add_child(p1)
pov_driving.add_child(p2)
pov_driving.add_child(p3)
pov_driving.add_child(p4)
pov_driving.add_child(p5)
pov_driving.add_child(p6)
pov_driving.add_child(p7)
pov_driving.add_child(p8)
##############################################
endcondition_part1 = StandStill(self.ego_vehicles[0],
name="StandStill")
endcondition_part2 = InTriggerDistanceToLocation(
self.ego_vehicles[0], self._target, dist_endcond)
endcondition = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
ego_driving = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_driving.add_child(
RssBasicAgentBehavior(self._rss_params, self.ego_vehicles[0],
self._ego_target_speed, self._target))
ego_driving.add_child(endcondition)
##############################################
parallel_drive = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(pov_driving)
parallel_drive.add_child(ego_driving)
seq = py_trees.composites.Sequence()
seq.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
seq.add_child(parallel_drive)
seq.add_child(ActorDestroy(self.other_actors[0]))
seq.add_child(ActorDestroy(self.ego_vehicles[0]))
return seq
def _create_behavior(self):
"""
Platooning behavior.
The lead vehicle travels for 100 meters.
A fleet of ego vehicles follows single-file behind the leader.
"""
lead_drive = py_trees.composites.Sequence("Lead Sequence")
lead_drive.add_child(
KeepVelocity(self.other_actors[0],
target_velocity=self._other_actor_target_speed,
distance=100))
ego_drive = py_trees.composites.Sequence("Ego Sequence")
ego_drive.add_child(
RssExtBehavior(self._rss_params,
self.ego_vehicles[0],
self._ego_target_speed,
self._target,
variant=self._variant))
platoon_1_drive = py_trees.composites.Sequence("Platoon 1 Sequence")
platoon_1_drive.add_child(
RssExtBehavior(self._rss_params,
self.other_actors[1],
self._platoon_speed,
self._target,
variant=self._variant))
platoon_2_drive = py_trees.composites.Sequence("Platoon 2 Sequence")
platoon_2_drive.add_child(
RssExtBehavior(self._rss_params,
self.other_actors[2],
self._platoon_speed,
self._target,
variant=self._variant))
parallel_drive = py_trees.composites.Parallel(
"All Cars Driving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(lead_drive)
parallel_drive.add_child(ego_drive)
parallel_drive.add_child(platoon_1_drive)
parallel_drive.add_child(platoon_2_drive)
# Build behavior tree
sequence = py_trees.composites.Sequence("All Behavior Sequence")
sequence.add_child(
ActorTransformSetter(self.other_actors[0], self._lead_transform))
sequence.add_child(
ActorTransformSetter(self.other_actors[1],
self._platoon_1_transform))
sequence.add_child(
ActorTransformSetter(self.other_actors[2],
self._platoon_2_transform))
sequence.add_child(TimeOut(1))
sequence.add_child(parallel_drive)
for actor in self.other_actors:
sequence.add_child(ActorDestroy(actor))
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('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):
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)
target_waypoint = generate_target_waypoint(
CarlaDataProvider.get_map().get_waypoint(
self.other_actors[0].get_location()), turn)
#print(target_waypoint)
#print('------------')
# Generating waypoint list till next intersection
wp_choice = target_waypoint.next(5.0)
while len(wp_choice) == 1:
target_waypoint = wp_choice[0]
#print(target_waypoint)
plan.append((target_waypoint, RoadOption.LANEFOLLOW))
wp_choice = target_waypoint.next(5.0)
#print(wp_choice[0])
#print(wp_choice[1])
continue_driving = py_trees.composites.Parallel(
"ContinueDriving",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
continue_driving.add_child(
WaypointFollower(self.other_actors[0],
self._other_actor_target_velocity,
plan=plan,
avoid_collision=False))
continue_driving.add_child(
DriveDistance(self.other_actors[0],
self._other_actor_distance,
name="Distance"))
continue_driving.add_child(TimeOut(10))
# finally wait that ego vehicle drove a specific distance
#wait = DriveDistance(self.ego_vehicles[0], self._ego_distance_to_drive, name="DriveDistance")
pov_driving = py_trees.composites.Sequence()
pov_driving.add_child(startcondition)
pov_driving.add_child(sync_arrival_parallel)
pov_driving.add_child(continue_driving)
#pov_driving.add_child(wait)
pov_driving.add_child(TimeOut(float('inf')))
ego_turn = -1 # turn left
_, target_waypoint = generate_target_waypoint_list(
CarlaDataProvider.get_map().get_waypoint(
self.ego_vehicles[0].get_location()), ego_turn)
# Generating waypoint list till next intersection
wp_choice = target_waypoint.next(30.0)
target_waypoint = wp_choice[0]
destination_location = target_waypoint.transform.location
#print(destination_location)
ego_driving = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
ego_driving.add_child(
RssBasicAgentBehavior(self._rss_params, self.ego_vehicles[0],
self._ego_target_speed,
destination_location))
ego_driving.add_child(
InTriggerDistanceToLocation(self.ego_vehicles[0],
destination_location, 15))
# Build behavior tree
parallel_drive = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
parallel_drive.add_child(pov_driving)
parallel_drive.add_child(ego_driving)
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
sequence.add_child(parallel_drive)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
"""
Only behavior here is to wait
"""
redundant = TimeOut(self.timeout - 5)
return redundant
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