def _get_target_waypoint(self): """ Gets the first waypoint after the junction. This method depends on the subtype of VehicleTurning scenario """ if self._subtype == 'right': return generate_target_waypoint(self._reference_waypoint, 1) elif self._subtype == 'left': return generate_target_waypoint(self._reference_waypoint, -1) elif self._subtype == 'route': return generate_target_waypoint_in_route(self._reference_waypoint, self._ego_route) else: raise ValueError("Trying to run a VehicleTurning scenario with a wrong subtype")
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 _initialize_actors(self, config): """ Custom initialization """ waypoint = self._reference_waypoint direction = self.SUBTYPE_INDEX_TRANSLATION[config.subtype] waypoint = generate_target_waypoint(waypoint, direction) _start_distance = 8 while True: wp_next = waypoint.get_right_lane() self._num_lane_changes += 1 if wp_next is not None: _start_distance += 1 waypoint = wp_next if waypoint.lane_type == carla.LaneType.Shoulder or waypoint.lane_type == carla.LaneType.Sidewalk: last_waypoint_lane = waypoint.lane_type break else: last_waypoint_lane = waypoint.lane_type break while True: try: self._other_actor_transform = get_opponent_transform( _start_distance, waypoint, self._trigger_location, last_waypoint_lane) first_vehicle = CarlaActorPool.request_new_actor( 'vehicle.diamondback.century', self._other_actor_transform) first_vehicle.set_simulate_physics(enabled=False) break except RuntimeError as r: # In the case there is an object just move a little bit and retry print("Base transform is blocking objects ", self._other_actor_transform) _start_distance += 0.2 self._spawn_attempted += 1 if self._spawn_attempted >= self._number_of_attempts: raise r # Set the transform to -500 z after we are able to spawn it actor_transform = carla.Transform( carla.Location(self._other_actor_transform.location.x, self._other_actor_transform.location.y, self._other_actor_transform.location.z - 500), self._other_actor_transform.rotation) first_vehicle.set_transform(actor_transform) first_vehicle.set_simulate_physics(enabled=False) self.other_actors.append(first_vehicle)
def _initialize_actors(self, config): """ Custom initialization """ # Get the waypoint right after the junction waypoint = generate_target_waypoint(self._reference_waypoint, -1) # Move a certain distance to the front start_distance = 8 waypoint = waypoint.next(start_distance)[0] # Get the last driving lane to the right waypoint, self._num_lane_changes = get_right_driving_lane(waypoint) # And for synchrony purposes, move to the front a bit added_dist = self._num_lane_changes while True: # Try to spawn the actor try: self._other_actor_transform = get_opponent_transform( added_dist, waypoint, self._trigger_location) first_vehicle = CarlaDataProvider.request_new_actor( 'vehicle.diamondback.century', self._other_actor_transform) first_vehicle.set_simulate_physics(enabled=False) break # Move the spawning point a bit and try again except RuntimeError as r: # In the case there is an object just move a little bit and retry print(" Base transform is blocking objects ", self._other_actor_transform) added_dist += 0.5 self._spawn_attempted += 1 if self._spawn_attempted >= self._number_of_attempts: raise r # Set the transform to -500 z after we are able to spawn it actor_transform = carla.Transform( carla.Location(self._other_actor_transform.location.x, self._other_actor_transform.location.y, self._other_actor_transform.location.z - 500), self._other_actor_transform.rotation) first_vehicle.set_transform(actor_transform) first_vehicle.set_simulate_physics(enabled=False) self.other_actors.append(first_vehicle)
def _initialize_actors(self, config): """ Custom initialization """ # Get the waypoint left after the junction waypoint = generate_target_waypoint(self._reference_waypoint, 1) while True: # Try to spawn the actor try: self._other_actor_transform = waypoint.transform first_vehicle = CarlaDataProvider.request_new_actor( 'vehicle.*', self._other_actor_transform) first_vehicle.set_simulate_physics(enabled=False) break # Move the spawning point a bit and try again except RuntimeError as r: # In the case there is an object just move a little bit and retry waypoint = waypoint.next(1)[0] self._spawn_attempted += 1 if self._spawn_attempted >= self._number_of_attempts: raise r # Set the transform to -500 z after we are able to spawn it actor_transform = carla.Transform( carla.Location(self._other_actor_transform.location.x, self._other_actor_transform.location.y, self._other_actor_transform.location.z - 500), self._other_actor_transform.rotation) first_vehicle.set_transform(actor_transform) first_vehicle.set_simulate_physics(enabled=False) self.other_actors.append(first_vehicle)
def get_route( self, spawn_location, distance: float = 10., turning_flag=-1, # left -> -1, straight -> 0, right -> 1 resolution: float = 1.): """ Generate a route for the scenario. Route is determined by class attribute turning_flag. params: distance: distance after the junction resolution: distance gap between waypoint in route. """ waypoint_route = [] transform_route = [] location_route = [] spawn_waypoint = self.map.get_waypoint( spawn_location, project_to_road=True, # must set to True, center of lane ) # start waypoint: beginning waypoint of the route, start_waypoint = spawn_waypoint.next(3.0)[ 0] # assuming that next waypoint is not in junction # exit_waypoint: the first waypoint after leaving the junction exit_waypoint = generate_target_waypoint(waypoint=start_waypoint, turn=turning_flag) # end_waypoint: end waypoint of the route end_waypoint, _ = get_waypoint_in_distance(exit_waypoint, distance) # list of carla.Location for route generation raw_route = [ start_waypoint.transform.location, exit_waypoint.transform.location, end_waypoint.transform.location, ] waypoint_route = interpolate_trajectory(world=self.world, waypoints_trajectory=raw_route, hop_resolution=resolution) # get route of transform for wp, road_option in waypoint_route: transform_route.append((wp.transform, road_option)) # get route of location for wp, road_option in waypoint_route: location_route.append((wp.transform.location, road_option)) # ==================== visualization ==================== key_waypoints = [ start_waypoint, exit_waypoint, end_waypoint, ] for wp in key_waypoints: draw_waypoint(self.world, wp, color=(red, red)) if self.verbose: # visualize complete route for wp, _ in waypoint_route: draw_waypoint(self.world, wp, color=(magenta, magenta)) return waypoint_route, location_route, transform_route
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 _initialize_actors(self, config): """ Custom initialization """ ego_location = config.trigger_points[0].location ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction starting_wp = ego_wp while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError( "Failed to find junction as a waypoint with no next was detected" ) starting_wp = starting_wps[0] junction = starting_wp.get_junction() # Get the opposite entry lane wp possible_directions = ['right', 'left'] self._rng.shuffle(possible_directions) for direction in possible_directions: entry_wps, _ = get_junction_topology(junction) source_entry_wps = filter_junction_wp_direction( starting_wp, entry_wps, direction) if source_entry_wps: self._direction = direction break if not self._direction: raise ValueError( "Trying to find a lane to spawn the opposite actor but none was found" ) # Get the source transform spawn_wp = source_entry_wps[0] added_dist = 0 while added_dist < self._source_dist: spawn_wps = spawn_wp.previous(1.0) if len(spawn_wps) == 0: raise ValueError( "Failed to find a source location as a waypoint with no previous was detected" ) if spawn_wps[0].is_junction: break spawn_wp = spawn_wps[0] added_dist += 1 self._spawn_wp = spawn_wp source_transform = spawn_wp.transform self._spawn_location = carla.Transform( source_transform.location + carla.Location(z=0.1), source_transform.rotation) # Spawn the actor and move it below ground opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards) opposite_actor = CarlaDataProvider.request_new_actor( opposite_bp_wildcard, self._spawn_location) if not opposite_actor: raise Exception("Couldn't spawn the actor") opposite_actor.set_light_state( carla.VehicleLightState(carla.VehicleLightState.Special1 | carla.VehicleLightState.Special2)) self.other_actors.append(opposite_actor) opposite_transform = carla.Transform( source_transform.location - carla.Location(z=500), source_transform.rotation) opposite_actor.set_transform(opposite_transform) opposite_actor.set_simulate_physics(enabled=False) # Get the sink location sink_exit_wp = generate_target_waypoint( self._map.get_waypoint(source_transform.location), 0) sink_wps = sink_exit_wp.next(self._sink_dist) if len(sink_wps) == 0: raise ValueError( "Failed to find a sink location as a waypoint with no next was detected" ) self._sink_wp = sink_wps[0] # get the collision location self._collision_location = get_geometric_linear_intersection( starting_wp.transform.location, source_entry_wps[0].transform.location) if not self._collision_location: raise ValueError("Couldn't find an intersection point") # Get the relevant traffic lights tls = self._world.get_traffic_lights_in_junction(junction.id) ego_tl = get_closest_traffic_light(ego_wp, tls) source_tl = get_closest_traffic_light( self._spawn_wp, tls, ) self._tl_dict = {} for tl in tls: if tl in (ego_tl, source_tl): self._tl_dict[tl] = carla.TrafficLightState.Green else: self._tl_dict[tl] = carla.TrafficLightState.Red
def _initialize_actors(self, config): """ Default initialization of other actors. Override this method in child class to provide custom initialization. """ ego_location = config.trigger_points[0].location ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location) # Get the junction starting_wp = ego_wp while not starting_wp.is_junction: starting_wps = starting_wp.next(1.0) if len(starting_wps) == 0: raise ValueError( "Failed to find junction as a waypoint with no next was detected" ) starting_wp = starting_wps[0] junction = starting_wp.get_junction() # Get the opposite entry lane wp entry_wps, _ = get_junction_topology(junction) source_entry_wps = filter_junction_wp_direction( starting_wp, entry_wps, self._direction) if not source_entry_wps: raise ValueError( "Trying to find a lane in the {} direction but none was found". format(self._direction)) # Get the source transform source_entry_wp = self._rng.choice(source_entry_wps) # Get the source transform source_wp = source_entry_wp accum_dist = 0 while accum_dist < self._source_dist: source_wps = source_wp.previous(5) if len(source_wps) == 0: raise ValueError( "Failed to find a source location as a waypoint with no previous was detected" ) if source_wps[0].is_junction: break source_wp = source_wps[0] accum_dist += 5 self._source_wp = source_wp source_transform = self._source_wp.transform # Get the sink location sink_exit_wp = generate_target_waypoint( self._map.get_waypoint(source_transform.location), 0) sink_wps = sink_exit_wp.next(self._sink_dist) if len(sink_wps) == 0: raise ValueError( "Failed to find a sink location as a waypoint with no next was detected" ) self._sink_wp = sink_wps[0] # get traffic lights tls = self._world.get_traffic_lights_in_junction(junction.id) ego_tl = get_closest_traffic_light(ego_wp, tls) source_tl = get_closest_traffic_light(self._source_wp, tls) self._flow_tl_dict = {} self._init_tl_dict = {} for tl in tls: if tl == ego_tl: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Red elif tl == source_tl: self._flow_tl_dict[tl] = carla.TrafficLightState.Green self._init_tl_dict[tl] = carla.TrafficLightState.Green else: self._flow_tl_dict[tl] = carla.TrafficLightState.Red self._init_tl_dict[tl] = carla.TrafficLightState.Red