def set_birds_eye_view_spectator(spectator: carla.Actor, followed_location: carla.Location, above: float): birds_eye_view = carla.Transform( carla.Location(x=followed_location.x, y=followed_location.y, z=above), carla.Rotation(pitch=-90), ) spectator.set_transform(birds_eye_view)
def set_spectator_above_actor(spectator: carla.Actor, transform: np.array) -> None: ''' Changes position of the spectator relative to actor position. :param spectator: :param transform: :return: ''' transform = numpy_to_transform(transform + [0, 0, 10, 0]) transform.rotation.pitch = -15 spectator.set_transform(transform)
def map_dynamic_actor(actor: carla.Actor, world_map: carla.Map) -> DynamicActor: location: carla.Location = actor.get_location() gnss: carla.GeoLocation = world_map.transform_to_geolocation(location) velocity: carla.Vector3D = actor.get_velocity() acceleration: carla.Vector3D = actor.get_acceleration() color: str = str( actor.attributes['color']) if 'color' in actor.attributes else None extent: carla.Vector3D = actor.bounding_box.extent transform: carla.Transform = carla.Transform( rotation=actor.get_transform().rotation) transformed_extent: Tuple[carla.Location, carla.Location, carla.Location, carla.Location] = ( transform.transform( carla.Location(+extent.x, +extent.y, 0)), transform.transform( carla.Location(+extent.x, -extent.y, 0)), transform.transform( carla.Location(-extent.x, -extent.y, 0)), transform.transform( carla.Location(-extent.x, +extent.y, 0)), ) bbox: Tuple[Point2D, Point2D, Point2D, Point2D] = cast( Tuple[Point2D, Point2D, Point2D, Point2D], tuple( map( lambda t: geoloc2point2d( world_map.transform_to_geolocation( carla.Vector3D(location.x + t.x, location.y + t.y, location.z))), transformed_extent))) return DynamicActor( id=actor.id, type=UncertainProperty(1., resolve_carla_type(actor.type_id)), type_id=actor.type_id, location=UncertainProperty(1., Point3D(location.x, location.y, location.z)), gnss=UncertainProperty( 1., Point3D(gnss.latitude, gnss.longitude, gnss.altitude)), dynamics=ActorDynamics(velocity=UncertainProperty( 1., Point3D(velocity.x, velocity.y, velocity.z)), acceleration=UncertainProperty( 1., Point3D(acceleration.x, acceleration.y, acceleration.z))), props=ActorProperties(color=UncertainProperty(1., color), extent=UncertainProperty( 1., Point3D(extent.x, extent.y, extent.z)), bbox=UncertainProperty(1., bbox)))
def compute_all_paths_for_actor( vehicle: carla.Actor, dist_between_waypoints: float, path_length: int = 10, lane_type: carla.LaneType = carla.LaneType.Driving, verbose: bool = False): world = vehicle.get_world() location = vehicle.get_location() paths = compute_all_paths_from_location(location, world, dist_between_waypoints, path_length, lane_type, verbose) return paths
def agent_vehicle_mask(self, agent: carla.Actor) -> Mask: canvas = self.make_empty_mask() bb = agent.bounding_box.extent corners = [ carla.Location(x=-bb.x, y=-bb.y), carla.Location(x=bb.x, y=-bb.y), carla.Location(x=bb.x, y=bb.y), carla.Location(x=-bb.x, y=bb.y), ] agent.get_transform().transform(corners) corners = [self.location_to_pixel(loc) for loc in corners] cv.fillPoly(img=canvas, pts=np.int32([corners]), color=COLOR_ON) return canvas
def actor_to_corners(actor: carla.Actor) -> Sequence[Tuple[float, float]]: # pylint: disable=no-member """Draws a rectangular for the bounding box of `actor`.""" bb = actor.bounding_box.extent # Draw actor as a rectangular. corners = [ carla.Location(x=-bb.x, y=-bb.y), # pylint: disable=no-member carla.Location(x=-bb.x, y=+bb.y), # pylint: disable=no-member carla.Location(x=+bb.x, y=+bb.y), # pylint: disable=no-member carla.Location(x=+bb.x, y=-bb.y), # pylint: disable=no-member ] # Convert to global coordinates. actor.get_transform().transform(corners) return corners
def apply_agent_following_transformation_to_masks( self, agent_vehicle: carla.Actor, masks: np.ndarray) -> np.ndarray: """Returns image of shape: height, width, channels""" agent_transform = agent_vehicle.get_transform() angle = (agent_transform.rotation.yaw + 90 ) # vehicle's front will point to the top # Rotating around the center crop_with_car_in_the_center = masks masks_n, h, w = crop_with_car_in_the_center.shape rotation_center = Coord(x=w // 2, y=h // 2) # warpAffine from OpenCV requires the first two dimensions to be in order: height, width, channels crop_with_centered_car = np.transpose(crop_with_car_in_the_center, axes=(1, 2, 0)) rotated = rotate(crop_with_centered_car, angle, center=rotation_center) half_width = self.target_size.width // 2 hslice = slice(rotation_center.x - half_width, rotation_center.x + half_width) if self._crop_type is BirdViewCropType.FRONT_AREA_ONLY: vslice = slice(rotation_center.y - self.target_size.height, rotation_center.y) elif self._crop_type is BirdViewCropType.FRONT_AND_REAR_AREA: half_height = self.target_size.height // 2 vslice = slice(rotation_center.y - half_height, rotation_center.y + half_height) else: raise NotImplementedError assert ( vslice.start > 0 and hslice.start > 0 ), "Trying to access negative indexes is not allowed, check for calculation errors!" car_on_the_bottom = rotated[vslice, hslice] return car_on_the_bottom
def produce(self, agent_vehicle: carla.Actor) -> BirdView: all_actors = actors.query_all(world=self._world) segregated_actors = actors.segregate_by_type(actors=all_actors) agent_vehicle_loc = agent_vehicle.get_location() # Reusing already generated static masks for whole map self.masks_generator.disable_local_rendering_mode() agent_global_px_pos = self.masks_generator.location_to_pixel( agent_vehicle_loc) cropping_rect = CroppingRect( x=int(agent_global_px_pos.x - self.rendering_area.width / 2), y=int(agent_global_px_pos.y - self.rendering_area.height / 2), width=self.rendering_area.width, height=self.rendering_area.height, ) masks = np.zeros( shape=( len(BirdViewMasks), self.rendering_area.height, self.rendering_area.width, ), dtype=np.uint8, ) masks[BirdViewMasks.ROAD.value] = self.full_road_cache[ cropping_rect.vslice, cropping_rect.hslice] masks[BirdViewMasks.LANES.value] = self.full_lanes_cache[ cropping_rect.vslice, cropping_rect.hslice] masks[BirdViewMasks.CENTERLINES.value] = self.full_centerlines_cache[ cropping_rect.vslice, cropping_rect.hslice] masks[BirdViewMasks.BUILDINGS.value] = self.full_buildings_cache[ cropping_rect.vslice, cropping_rect.hslice] # Dynamic masks rendering_window = RenderingWindow(origin=agent_vehicle_loc, area=self.rendering_area) self.masks_generator.enable_local_rendering_mode(rendering_window) masks = self._render_actors_masks(agent_vehicle, segregated_actors, masks) cropped_masks = self.apply_agent_following_transformation_to_masks( agent_vehicle, masks) ordered_indices = [ mask.value for mask in BirdViewMasks.bottom_to_top() ] return cropped_masks[:, :, ordered_indices]
def curved_road_segments_enclosure_from_actor( self, actor: carla.Actor, max_distance: float, choices=[], flip_x=False, flip_y=False) -> util.AttrDict: """Get segmented enclosure of fixed size from curved road. Parameters ========== actor : carla.Actor Position of actor to get starting waypoint. max_distance : float The max distance of the path starting from `start_wp`. Use to specify length of path. choices : list of int The indices of the turns to make when reaching a fork on the road network. `choices[0]` is the index of the first turn, `choices[1]` is the index of the second turn, etc. Returns ======= util.AttrDict Container of road segment properties. - spline : scipy.interpolate.CubicSpline The spline representing the path the vehicle should motion plan on. - polytopes : list of (ndarray, ndarray) List of polytopes in H-representation (A, b) where x is in polytope if Ax <= b. - distances : ndarray The distances along the spline to follow from nearest endpoint before encountering corresponding covering polytope in index. - positions : ndarray The 2D positions of center of the covering polytope in index. """ t = actor.get_transform() wp = self.carla_map.get_waypoint(t.location) wp = wp.previous(5)[0] lane_width = wp.lane_width return cover_along_waypoints_fixedsize(wp, choices, max_distance + 7, lane_width, flip_x=flip_x, flip_y=flip_y)
def getActorWorldBBox(actor: carla.Actor): actorBBox = actor.bounding_box cords = np.zeros((8, 4)) # Get the box extent extent = actorBBox.extent cords[0, :] = np.array([extent.x, extent.y, -extent.z, 1]) cords[1, :] = np.array([-extent.x, extent.y, -extent.z, 1]) cords[2, :] = np.array([-extent.x, -extent.y, -extent.z, 1]) cords[3, :] = np.array([extent.x, -extent.y, -extent.z, 1]) cords[4, :] = np.array([extent.x, extent.y, extent.z, 1]) cords[5, :] = np.array([-extent.x, extent.y, extent.z, 1]) cords[6, :] = np.array([-extent.x, -extent.y, extent.z, 1]) cords[7, :] = np.array([extent.x, -extent.y, extent.z, 1]) bb_transform = carla.Transform(actorBBox.location) bb_matrix = get_matrix(bb_transform) actor_world_matrix = get_matrix(actor.get_transform()) bb_world_matrix = np.dot(actor_world_matrix, bb_matrix) world_cords = np.dot(bb_world_matrix, np.transpose(cords)) return world_cords
def __init__(self, ego_veh: carla.Actor, gt_config: dict, debug=False): """ Constructor method. Input: ego_veh: Carla.Actor obj of the ego vehicle. Carla.Actor provides a get_world() method to get the Carla.World it belongs to; thus, ego vehicle actor is sufficient to retrieve other info. gt_config (dict): Configurations. debug (bool): True to turn on debug features. """ self.debug = debug # Retrieve carla.World object from ego vehicle carla.Actor object carla_world = ego_veh.get_world() # Dict as an buffer to store all ground truth data of interest # Using dict helps automate data selection during recording since data can be queried by keys # This buffer is automatically updated when the child ground truth extractors update their buffer self.all_gt = {'static': {}, 'seq': {}} # Traffic signs self.all_gt['static']['traffic_sign'] = self.get_traffic_signs( carla_world, self.debug) # Front bumper's transform in Carla's coordinate system # It's for the convenience of querying waypoints for lane using carla's APIs # TODO: Try put this frame's origin at the intersection of camera FOV and ground surface? self._fbumper_carla_tform = None # Pose ground truth extractor self.pose_gt = PoseGTExtractor(ego_veh, gt_config['pose']) # Lane ground truth extractor self.lane_gt = LaneGTExtractor(carla_world, gt_config['lane'], self.debug) # Set up buffer for sequential data # Refer to pose ground truth extractor's gt buffer self.all_gt['seq']['pose'] = self.pose_gt.gt # Refer to lane ground truth extractor's gt buffer self.all_gt['seq']['lane'] = self.lane_gt.gt
def road_boundary_constraints_from_actor( self, actor: carla.Actor, max_distance: float, choices=[], flip_x=False, flip_y=False) -> RoadBoundaryConstraint: """Get segmented enclosure of fixed size from curved road. Parameters ========== actor : carla.Actor Position of actor to get starting waypoint. max_distance : float The max distance of the path starting from `start_wp`. Use to specify length of path. choices : list of int The indices of the turns to make when reaching a fork on the road network. `choices[0]` is the index of the first turn, `choices[1]` is the index of the second turn, etc. Returns ======= RoadBoundaryConstraint The road boundary constraints. TODO: MapQuerier.road_segment_enclosure_from_actor() and MapQuerier.curved_road_segments_enclosure_from_actor() should be refactored to use RoadBoundaryConstraint as a factory. """ t = actor.get_transform() wp = self.carla_map.get_waypoint(t.location) wp = wp.previous(5)[0] lane_width = wp.lane_width return RoadBoundaryConstraint(wp, max_distance + 7, lane_width, choices, flip_x=flip_x, flip_y=flip_y)
def __init__(self, ego_veh: carla.Actor, pose_gt_config: dict): # Ego vehicle self.ego_veh = ego_veh self.ego_veh_tform = ego_veh.get_transform() # Distance from rear axle to front bumper self.raxle_to_fbumper = pose_gt_config['raxle_to_fbumper'] # Distance from rear axle to center of gravity self.raxle_to_cg = pose_gt_config['raxle_to_cg'] self.gt = {} # Rear axle in Carla's coordinate system (left-handed z-up) as a carla.Vector3D object raxle_location = self.ego_veh_tform.transform( carla.Location(x=-self.raxle_to_cg)) # Rear axle's location in our coordinate system (right-handed z-up) as a numpy array # This is the ground truth of the rear axle's location self.gt['raxle_location'] = np.array( [raxle_location.x, -raxle_location.y, raxle_location.z]) # Rear axle's orientation in our coordinate system (right-handed z-up) as a numpy array (roll, pitch, yaw) # This is the ground truth of the rear axle's orientation (rad) self.gt['raxle_orientation'] = np.array([ self.ego_veh_tform.rotation.roll, -self.ego_veh_tform.rotation.pitch, -self.ego_veh_tform.rotation.yaw ]) * np.pi / 180
def get_speed(actor: carla.Actor) -> float: """in km/h""" vector: carla.Vector3D = actor.get_velocity() MPS_TO_KMH = 3.6 return MPS_TO_KMH * math.sqrt(vector.x**2 + vector.y**2 + vector.z**2)
def speed(actor: carla.Actor) -> float: """Returns the speed of the given actor in km/h.""" return 3.6 * vector_norm(actor.get_velocity())