def _create_behavior(self): """ The behavior tree returned by this method is as follows: The ego vehicle is trying to pass a leading vehicle in the same lane by moving onto the oncoming lane while another vehicle is moving in the opposite direction in the oncoming lane. """ # Leaf nodes actor_source = ActorSource( [ "vehicle.audi.tt", "vehicle.tesla.model3", "vehicle.nissan.micra" ], self._source_transform, self._source_gap, self._blackboard_queue_name, ) actor_sink = ActorSink(self._sink_location, 10) ego_drive_distance = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_drive_distance) waypoint_follower = WaypointFollower( self.other_actors[1], self._opposite_speed, blackboard_queue_name=self._blackboard_queue_name, avoid_collision=True, ) pedestrian_walking = KeepVelocity(self.other_actors[2], 1, distance=300) # Non-leaf nodes parallel_root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # Building tree parallel_root.add_child(ego_drive_distance) parallel_root.add_child(actor_source) parallel_root.add_child(actor_sink) parallel_root.add_child(waypoint_follower) parallel_root.add_child(pedestrian_walking) scenario_sequence = py_trees.composites.Sequence() scenario_sequence.add_child( ActorTransformSetter(self.other_actors[0], self._first_actor_transform)) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[1], self._second_actor_transform)) # scenario_sequence.add_child( # ActorTransformSetter(self.other_actors[2], # self._third_actor_transform)) scenario_sequence.add_child(parallel_root) scenario_sequence.add_child(ActorDestroy(self.other_actors[0])) scenario_sequence.add_child(ActorDestroy(self.other_actors[1])) scenario_sequence.add_child(ActorDestroy(self.other_actors[2])) return scenario_sequence
def _create_behavior(self): """ The scenario defined after is a "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): # 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): """ The scenario defined after is a "control loss vehicle" scenario. After invoking this scenario, it will wait until the vehicle drove a few meters (_start_distance), and then perform a jitter action. Finally, the vehicle has to reach a target point (_end_distance). If this does not happen within 60 seconds, a timeout stops the scenario """ # start condition start_end_parallel = py_trees.composites.Parallel("Jitter", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) start_condition = InTriggerDistanceToLocation(self.ego_vehicles[0], self.first_loc_prev, self._trigger_dist) for _ in range(self._no_of_jitter): # change the current noise to be applied turn = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise, self._noise_mean, self._noise_std, self._dynamic_mean_for_steer, self._dynamic_mean_for_throttle) # Mean value of steering noise # Noise end! put again the added noise to zero. noise_end = ChangeNoiseParameters(self._current_steer_noise, self._current_throttle_noise, 0, 0, 0, 0) jitter_action = py_trees.composites.Parallel("Jitter", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # Abort jitter_sequence, if the vehicle is approaching an intersection jitter_abort = InTriggerDistanceToNextIntersection(self.ego_vehicles[0], self._abort_distance_to_intersection) # endcondition: Check if vehicle reached waypoint _end_distance from here: end_condition = DriveDistance(self.ego_vehicles[0], self._end_distance) start_end_parallel.add_child(start_condition) start_end_parallel.add_child(end_condition) # Build behavior tree sequence = py_trees.composites.Sequence("ControlLoss") sequence.add_child(ActorTransformSetter(self.other_actors[0], self.first_transform, physics=False)) sequence.add_child(ActorTransformSetter(self.other_actors[1], self.sec_transform, physics=False)) sequence.add_child(ActorTransformSetter(self.other_actors[2], self.third_transform, physics=False)) jitter = py_trees.composites.Sequence("Jitter Behavior") jitter.add_child(turn) jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.sec_loc_prev, self._trigger_dist)) jitter.add_child(turn) jitter.add_child(InTriggerDistanceToLocation(self.ego_vehicles[0], self.third_loc_prev, self._trigger_dist)) jitter.add_child(turn) jitter_action.add_child(jitter) jitter_action.add_child(jitter_abort) sequence.add_child(start_end_parallel) sequence.add_child(jitter_action) sequence.add_child(end_condition) sequence.add_child(noise_end) return sequence
def _create_behavior(self): """ 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): """ 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): """ 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): """ Only behavior here is to wait """ lane_width = self.ego_vehicles[0].get_world().get_map().get_waypoint( self.ego_vehicles[0].get_location()).lane_width lane_width = lane_width + (1.25 * lane_width) # leaf nodes actor_stand = TimeOut(15) actor_removed = ActorDestroy(self.other_actors[0]) end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven) # non leaf nodes root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) scenario_sequence = py_trees.composites.Sequence() # building tree root.add_child(scenario_sequence) scenario_sequence.add_child( ActorTransformSetter(self.other_actors[0], self.transform)) scenario_sequence.add_child(actor_stand) scenario_sequence.add_child(actor_removed) scenario_sequence.add_child(end_condition) return root
def _create_behavior(self): """ 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): """ Hero vehicle is turning left in an urban area, at a signalized intersection, while other actor coming straight .The hero actor may turn left either before other actor passes intersection or later, without any collision. After 80 seconds, a timeout stops the scenario. """ sequence = py_trees.composites.Sequence("Sequence Behavior") # Selecting straight path at intersection target_waypoint = generate_target_waypoint( CarlaDataProvider.get_map().get_waypoint( self.other_actors[0].get_location()), 0) # Generating waypoint list till next intersection plan = [] wp_choice = target_waypoint.next(1.0) while not wp_choice[0].is_intersection: target_waypoint = wp_choice[0] plan.append((target_waypoint, RoadOption.LANEFOLLOW)) wp_choice = target_waypoint.next(1.0) # adding flow of actors actor_source = ActorSource(['vehicle.tesla.model3', 'vehicle.audi.tt'], self._other_actor_transform, 15, self._blackboard_queue_name) # destroying flow of actors actor_sink = ActorSink(plan[-1][0].transform.location, 10) # follow waypoints untill next intersection move_actor = WaypointFollower( self.other_actors[0], self._target_vel, plan=plan, blackboard_queue_name=self._blackboard_queue_name, avoid_collision=True) # wait wait = DriveDistance(self.ego_vehicles[0], self._ego_distance) # Behavior tree root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(wait) root.add_child(actor_source) root.add_child(actor_sink) root.add_child(move_actor) sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(root) sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence
def _create_behavior(self): """ Order of sequence: - car_visible: spawn car at a visible transform - just_drive: drive until in trigger distance to ego_vehicle - accelerate: accelerate to catch up distance to ego_vehicle - lane_change: change the lane - endcondition: drive for a defined distance """ # car_visible behaviour = py_trees.composites.Sequence("CarOn_{}_Lane" .format(self._direction)) car_visible = ActorTransformSetter(self.other_actors[0], self._transform_visible) behaviour.add_child(car_visible) # just_drive just_drive = py_trees.composites.Parallel( "DrivingStraight", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) car_driving = WaypointFollower(self.other_actors[0], self._velocity) just_drive.add_child(car_driving) trigger_distance = InTriggerDistanceToVehicle( self.other_actors[0], self.ego_vehicles[0], self._trigger_distance) just_drive.add_child(trigger_distance) behaviour.add_child(just_drive) # accelerate accelerate = AccelerateToCatchUp(self.other_actors[0], self.ego_vehicles[0], throttle_value=1, delta_velocity=self._delta_velocity, trigger_distance=5, max_distance=500) behaviour.add_child(accelerate) # lane_change if self._direction == 'left': lane_change = LaneChange( self.other_actors[0], speed=None, direction='right', distance_same_lane=5, distance_other_lane=300) behaviour.add_child(lane_change) else: lane_change = LaneChange( self.other_actors[0], speed=None, direction='left', distance_same_lane=5, distance_other_lane=300) behaviour.add_child(lane_change) # endcondition endcondition = DriveDistance(self.other_actors[0], 200) # build tree root = py_trees.composites.Sequence("Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(behaviour) root.add_child(endcondition) return root
def _create_behavior(self): """ Hero vehicle is entering a junction in an urban area, at a signalized intersection, while another actor runs a red lift, forcing the ego to break. """ sequence = py_trees.composites.Sequence() # Wait until ego is close to the adversary trigger_adversary = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="TriggerAdversaryStart") trigger_adversary.add_child( InTimeToArrivalToLocation(self.ego_vehicles[0], self._sync_time, self._collision_location)) trigger_adversary.add_child( InTriggerDistanceToLocation(self.ego_vehicles[0], self._collision_location, self._min_trigger_dist)) sequence.add_child(trigger_adversary) sequence.add_child( BasicAgentBehavior( self.other_actors[0], target_location=self._sink_wp.transform.location, target_speed=self._adversary_speed, opt_dict={'ignore_vehicles': True}, name="AdversaryCrossing")) main_behavior = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) main_behavior.add_child(TrafficLightFreezer(self._tl_dict)) main_behavior.add_child(sequence) root = py_trees.composites.Sequence() if CarlaDataProvider.get_ego_vehicle_route(): root.add_child(Scenario7Manager(self._direction)) root.add_child( ActorTransformSetter(self.other_actors[0], self._spawn_location)) root.add_child(main_behavior) root.add_child(ActorDestroy(self.other_actors[0])) root.add_child(WaitEndIntersection(self.ego_vehicles[0])) return root
def _create_behavior(self): """ Scenario behavior: The other vehicle waits until the ego vehicle is close enough to the intersection and that its own traffic light is red. Then, it will start driving and 'illegally' cross the intersection. After a short distance it should stop again, outside of the intersection. The ego vehicle has to avoid the crash, but continue driving after the intersection is clear. If this does not happen within 120 seconds, a timeout stops the scenario """ crossing_point_dynamic = get_crossing_point(self.ego_vehicles[0]) # start condition startcondition = InTriggerDistanceToLocation( self.ego_vehicles[0], crossing_point_dynamic, self._ego_distance_to_traffic_light, name="Waiting for start position") sync_arrival_parallel = py_trees.composites.Parallel( "Synchronize arrival times", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) location_of_collision_dynamic = get_geometric_linear_intersection( self.ego_vehicles[0], self.other_actors[0]) sync_arrival = SyncArrival(self.other_actors[0], self.ego_vehicles[0], location_of_collision_dynamic) sync_arrival_stop = InTriggerDistanceToNextIntersection( self.other_actors[0], 5) sync_arrival_parallel.add_child(sync_arrival) sync_arrival_parallel.add_child(sync_arrival_stop) # Generate plan for WaypointFollower turn = 0 # drive straight ahead plan = [] # generating waypoints until intersection (target_waypoint) plan, target_waypoint = generate_target_waypoint_list( CarlaDataProvider.get_map().get_waypoint( self.other_actors[0].get_location()), turn) # Generating waypoint list till next intersection wp_choice = target_waypoint.next(5.0) while len(wp_choice) == 1: target_waypoint = wp_choice[0] plan.append((target_waypoint, RoadOption.LANEFOLLOW)) wp_choice = target_waypoint.next(5.0) continue_driving = py_trees.composites.Parallel( "ContinueDriving", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) continue_driving_waypoints = WaypointFollower( self.other_actors[0], self._other_actor_target_velocity, plan=plan, avoid_collision=False) continue_driving_distance = DriveDistance(self.other_actors[0], self._other_actor_distance, name="Distance") continue_driving_timeout = TimeOut(10) continue_driving.add_child(continue_driving_waypoints) continue_driving.add_child(continue_driving_distance) continue_driving.add_child(continue_driving_timeout) # finally wait that ego vehicle drove a specific distance wait = DriveDistance(self.ego_vehicles[0], self._ego_distance_to_drive, name="DriveDistance") # Build behavior tree sequence = py_trees.composites.Sequence("Sequence Behavior") sequence.add_child( ActorTransformSetter(self.other_actors[0], self._other_actor_transform)) sequence.add_child(startcondition) sequence.add_child(sync_arrival_parallel) sequence.add_child(continue_driving) sequence.add_child(wait) sequence.add_child(ActorDestroy(self.other_actors[0])) return sequence
def _create_behavior(self): """ The scenario defined after is a "other leading vehicle" scenario. After invoking this scenario, the user controlled vehicle has to drive towards the moving other actors, then make the leading actor to decelerate when user controlled vehicle is at some close distance. Finally, the user-controlled vehicle has to change lane to avoid collision and follow other leading actor in other lane to end the scenario. If this does not happen within 90 seconds, a timeout stops the scenario or the ego vehicle drives certain distance and stops the scenario. """ # start condition driving_in_same_direction = py_trees.composites.Parallel( "All actors driving in same direction", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) leading_actor_sequence_behavior = py_trees.composites.Sequence( "Decelerating actor sequence behavior") # both actors moving in same direction keep_velocity = py_trees.composites.Parallel( "Trigger condition for deceleration", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) keep_velocity.add_child( WaypointFollower(self.other_actors[0], self._first_vehicle_speed, avoid_collision=True)) keep_velocity.add_child( InTriggerDistanceToVehicle(self.other_actors[0], self.ego_vehicles[0], 55)) # Decelerating actor sequence behavior decelerate = self._first_vehicle_speed / 3.2 leading_actor_sequence_behavior.add_child(keep_velocity) leading_actor_sequence_behavior.add_child( WaypointFollower(self.other_actors[0], decelerate, avoid_collision=True)) # end condition ego_drive_distance = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_drive_distance) # Build behavior tree sequence = py_trees.composites.Sequence("Scenario behavior") parallel_root = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) parallel_root.add_child(ego_drive_distance) parallel_root.add_child(driving_in_same_direction) driving_in_same_direction.add_child(leading_actor_sequence_behavior) driving_in_same_direction.add_child( WaypointFollower(self.other_actors[1], self._second_vehicle_speed, avoid_collision=True)) sequence.add_child( ActorTransformSetter(self.other_actors[0], self._first_actor_transform)) sequence.add_child( ActorTransformSetter(self.other_actors[1], self._second_actor_transform)) sequence.add_child(parallel_root) sequence.add_child(ActorDestroy(self.other_actors[0])) sequence.add_child(ActorDestroy(self.other_actors[1])) return sequence
def _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): """ The scenario defined after is a "follow leading vehicle" scenario. After invoking this scenario, it will wait for the user controlled vehicle to enter the start region, then make the other actor to drive until reaching the next intersection. Finally, the user-controlled vehicle has to be close enough to the other actor to end the scenario. If this does not happen within 60 seconds, a timeout stops the scenario """ root = py_trees.composites.Parallel( "Running random scenario", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) root.add_child(StandStill(self.ego_vehicles[0], "scenario ended", 3.0)) root.add_child( InTriggerDistanceToLocation(self.ego_vehicles[0], carla.Location(200, -250, 0), 10)) for (scenario, actor_dict) in self.scenario_list: if scenario == "VehiclesAhead": wait_for_trigger = InTriggerDistanceToLocation( self.ego_vehicles[0], actor_dict.trigger.transform.location, 10) start_transform = ActorTransformSetter( self.other_actors[actor_dict.index], actor_dict.start_transform) keep_driving = WaypointFollower( self.other_actors[actor_dict.index], actor_dict.speed, avoid_collision=True) drive = py_trees.composites.Parallel( "Driving for a distance", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) drive_success = DriveDistance( self.other_actors[actor_dict.index], random.randint(50, 100)) drive.add_child(drive_success) drive.add_child(keep_driving) stand = StandStill(self.other_actors[actor_dict.index], "stand still", random.randint(1, 5)) stop = StopVehicle(self.other_actors[actor_dict.index], 1.0) accelerate = AccelerateToVelocity( self.other_actors[actor_dict.index], 1.0, actor_dict.speed) # Build behavior tree sequence = py_trees.composites.Sequence( "VehiclesAhead behavior sequence") sequence.add_child(wait_for_trigger) sequence.add_child(start_transform) # stop_condition = py_trees.composites.Parallel("stop condition", # policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) # stop_condition.add_child(keep_driving) # stop_condition.add_child(drive_success) if actor_dict.stop: sequence.add_child(drive) sequence.add_child(stop) sequence.add_child(accelerate) sequence.add_child(keep_driving) #stop_condition print("lead vehicle stop behavior added") else: sequence.add_child(keep_driving) #stop_condition sequence.add_child( ActorDestroy(self.other_actors[actor_dict.index])) root.add_child(sequence) elif scenario == "StationaryObstaclesAhead": lane_width = self.ego_vehicles[0].get_world().get_map( ).get_waypoint(self.ego_vehicles[0].get_location()).lane_width lane_width = lane_width + (1.25 * lane_width) sequence = py_trees.composites.Sequence( "StationaryObstaclesAhead behavior sequence") # actor_stand = TimeOut(15) # actor_removed = ActorDestroy(self.other_actors[num_actors]) # end_condition = DriveDistance(self.ego_vehicles[0], self._ego_vehicle_distance_driven) sequence.add_child( ActorTransformSetter(self.other_actors[actor_dict.index], actor_dict.start_transform)) sequence.add_child(TimeOut(120)) root.add_child(sequence) elif scenario == "CutIn": sequence = py_trees.composites.Sequence("CarOn_{}_Lane".format( actor_dict.direction)) car_visible = ActorTransformSetter( self.other_actors[actor_dict.index], actor_dict.start_transform) sequence.add_child( InTriggerDistanceToLocation( self.ego_vehicles[0], actor_dict.trigger.transform.location, 10)) sequence.add_child(car_visible) # accelerate accelerate = AccelerateToCatchUp( self.other_actors[actor_dict.index], self.ego_vehicles[0], throttle_value=1, delta_velocity=actor_dict.delta_speed, trigger_distance=actor_dict.trigger_distance, max_distance=100) sequence.add_child(accelerate) # lane_change # lane_change = None if actor_dict.direction == 'left': lane_change = LaneChange( self.other_actors[actor_dict.index], speed=actor_dict.lane_change_speed, direction='right', distance_same_lane=50, distance_other_lane=10) sequence.add_child(lane_change) else: lane_change = LaneChange( self.other_actors[actor_dict.index], speed=actor_dict.lane_change_speed, direction='left', distance_same_lane=50, distance_other_lane=10) sequence.add_child(lane_change) sequence.add_child( WaypointFollower(self.other_actors[actor_dict.index], target_speed=20, avoid_collision=True)) endcondition = DriveDistance( self.other_actors[actor_dict.index], 150) parallel = py_trees.composites.Sequence( "Behavior", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE) parallel.add_child(sequence) parallel.add_child(endcondition) root.add_child(parallel) elif scenario == "DynamicObstaclesAhead": lane_width = actor_dict.trigger.lane_width lane_width = lane_width + (1.25 * lane_width * actor_dict.num_lane_changes) start_condition = InTimeToArrivalToVehicle( self.other_actors[actor_dict.index], self.ego_vehicles[0], actor_dict.time_to_reach) # actor_velocity = KeepVelocity(self.other_actors[actor_dict.index], # actor_dict.speed, # name="walker velocity") # actor_drive = DriveDistance(self.other_actors[actor_dict.index], # 0.5 * lane_width, # name="walker drive distance") actor_start_cross_lane = AccelerateToVelocity( self.other_actors[actor_dict.index], 1.0, actor_dict.speed, name="walker crossing lane accelerate velocity") actor_cross_lane = DriveDistance( self.other_actors[actor_dict.index], lane_width, name="walker drive distance for lane crossing ") actor_stop_cross_lane = StopVehicle( self.other_actors[actor_dict.index], 1.0, name="walker stop") ego_pass_machine = DriveDistance( self.ego_vehicles[0], 5, name="ego vehicle passed walker") # actor_remove = ActorDestroy(self.other_actors[actor_dict.index], # name="Destroying walker") scenario_sequence = py_trees.composites.Sequence() keep_velocity_other = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="keep velocity other") keep_velocity = py_trees.composites.Parallel( policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE, name="keep velocity") scenario_sequence.add_child( ActorTransformSetter(self.other_actors[actor_dict.index], actor_dict.start_transform, name='TransformSetterTS3walker', physics=False)) # scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], True)) scenario_sequence.add_child(start_condition) # scenario_sequence.add_child(HandBrakeVehicle(self.other_actors[actor_dict.index], False)) # scenario_sequence.add_child(keep_velocity) scenario_sequence.add_child(keep_velocity_other) scenario_sequence.add_child(actor_stop_cross_lane) # keep_velocity.add_child(actor_velocity) # keep_velocity.add_child(actor_drive) keep_velocity_other.add_child(actor_start_cross_lane) # keep_velocity_other.add_child(actor_cross_lane) # keep_velocity_other.add_child(ego_pass_machine) root.add_child(scenario_sequence) return root
def _create_behavior(self): """ 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