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):
"""
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):
"""
After invoking this scenario, it will wait for the user
controlled vehicle to enter the start region,
then make a traffic participant to accelerate
until it is going fast enough to reach an intersection point.
at the same time as the user controlled vehicle at the junction.
Once the user controlled vehicle comes close to the junction,
the traffic participant accelerates and passes through the junction.
After 60 seconds, a timeout stops the scenario.
"""
# Creating leaf nodes
start_other_trigger = InTriggerRegion(self.ego_vehicles[0], -80, -70,
-75, -60)
sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0],
carla.Location(x=-74.63, y=-136.34))
pass_through_trigger = InTriggerRegion(self.ego_vehicles[0], -90, -70,
-124, -119)
keep_velocity_other = KeepVelocity(self.other_actors[0],
self._other_actor_target_velocity)
stop_other_trigger = InTriggerRegion(self.other_actors[0], -45, -35,
-140, -130)
stop_other = StopVehicle(self.other_actors[0],
self._other_actor_max_brake)
end_condition = InTriggerRegion(self.ego_vehicles[0], -90, -70, -170,
-156)
# Creating non-leaf nodes
root = py_trees.composites.Sequence()
scenario_sequence = py_trees.composites.Sequence()
sync_arrival_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
keep_velocity_other_parallel = py_trees.composites.Parallel(
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
# Building tree
root.add_child(scenario_sequence)
scenario_sequence.add_child(
ActorTransformSetter(self.other_actors[0],
self._other_actor_transform))
scenario_sequence.add_child(start_other_trigger)
scenario_sequence.add_child(sync_arrival_parallel)
scenario_sequence.add_child(keep_velocity_other_parallel)
scenario_sequence.add_child(stop_other)
scenario_sequence.add_child(end_condition)
scenario_sequence.add_child(ActorDestroy(self.other_actors[0]))
sync_arrival_parallel.add_child(sync_arrival)
sync_arrival_parallel.add_child(pass_through_trigger)
keep_velocity_other_parallel.add_child(keep_velocity_other)
keep_velocity_other_parallel.add_child(stop_other_trigger)
return root
def _create_behavior(self):
"""
The scenario defined after is a "follow leading vehicle" scenario. After
invoking this scenario, it will wait for the user controlled vehicle to
enter the start region, then make the other actor to drive until reaching
the next intersection. Finally, the user-controlled vehicle has to be close
enough to the other actor to end the scenario.
If this does not happen within 60 seconds, a timeout stops the scenario
"""
# to avoid the other actor blocking traffic, it was spawed elsewhere
# reset its pose to the required one
start_transform = ActorTransformSetter(self.other_actors[0],
self._other_actor_transform)
# let the other actor drive until next intersection
# @todo: We should add some feedback mechanism to respond to ego_vehicle behavior
driving_to_next_intersection = py_trees.composites.Parallel(
"DrivingTowardsIntersection",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
driving_to_next_intersection.add_child(
WaypointFollower(self.other_actors[0], self._first_vehicle_speed))
driving_to_next_intersection.add_child(
InTriggerDistanceToNextIntersection(
self.other_actors[0],
self._other_actor_stop_in_front_intersection))
# stop vehicle
stop = StopVehicle(self.other_actors[0], self._other_actor_max_brake)
# end condition
endcondition = py_trees.composites.Parallel(
"Waiting for end position",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[0],
self.ego_vehicles[0],
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0],
name="StandStill",
duration=1)
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
# Build behavior tree
sequence = py_trees.composites.Sequence("Sequence Behavior")
sequence.add_child(start_transform)
sequence.add_child(driving_to_next_intersection)
sequence.add_child(stop)
sequence.add_child(endcondition)
sequence.add_child(ActorDestroy(self.other_actors[0]))
return sequence
def _create_behavior(self):
# sequence vw
# make visible
sequence_vw = py_trees.composites.Sequence("VW T2")
vw_visible = ActorTransformSetter(self.other_actors[1], self.slow_car_visible)
sequence_vw.add_child(vw_visible)
# brake, avoid rolling backwarts
brake = StopVehicle(self.other_actors[1], self._max_brake)
sequence_vw.add_child(brake)
sequence_vw.add_child(Idle())
# sequence tesla
# make visible
sequence_tesla = py_trees.composites.Sequence("Tesla")
tesla_visible = ActorTransformSetter(self.other_actors[0], self.fast_car_visible)
sequence_tesla.add_child(tesla_visible)
# drive fast towards slow vehicle
just_drive = py_trees.composites.Parallel("DrivingTowardsSlowVehicle",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
tesla_driving_fast = WaypointFollower(self.other_actors[0], self._fast_vehicle_velocity)
just_drive.add_child(tesla_driving_fast)
distance_to_vehicle = InTriggerDistanceToVehicle(
self.other_actors[1], self.other_actors[0], self._trigger_distance)
just_drive.add_child(distance_to_vehicle)
sequence_tesla.add_child(just_drive)
# change lane
lane_change_atomic = LaneChange(self.other_actors[0], distance_other_lane=200)
sequence_tesla.add_child(lane_change_atomic)
sequence_tesla.add_child(Idle())
# ego vehicle
# end condition
endcondition = py_trees.composites.Parallel("Waiting for end position of ego vehicle",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ALL)
endcondition_part1 = InTriggerDistanceToVehicle(self.other_actors[1],
self.ego_vehicles[0],
distance=20,
name="FinalDistance")
endcondition_part2 = StandStill(self.ego_vehicles[0], name="FinalSpeed")
endcondition.add_child(endcondition_part1)
endcondition.add_child(endcondition_part2)
# build tree
root = py_trees.composites.Parallel("Parallel Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
root.add_child(sequence_vw)
root.add_child(sequence_tesla)
root.add_child(endcondition)
return root
def _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
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