def process_depth_images(msg, depth_camera_setup, ego_vehicle, speed, csv, surface, visualize=False): print("Received a message for the time: {}".format(msg.timestamp)) # If we are in distance to the destination, stop and exit with success. if ego_vehicle.get_location().distance(VEHICLE_DESTINATION) <= 5: ego_vehicle.set_velocity(carla.Vector3D()) CLEANUP_FUNCTION() sys.exit(0) # Get the RGB image corresponding to the given depth image timestamp. rgb_image = retrieve_rgb_image(msg.timestamp) # Visualize the image and the bounding boxes if needed. if visualize: draw_image(rgb_image, surface) # Transform the pedestrians into the detected objects relative to the # current frame. bb_surface = None resolution = (depth_camera_setup.width, depth_camera_setup.height) if visualize: bb_surface = pygame.Surface(resolution) bb_surface.set_colorkey((0, 0, 0)) detected_pedestrians = [] for pedestrian in ego_vehicle.get_world().get_actors().filter('walker.*'): bbox = get_2d_bbox_from_3d_box( depth_to_array(msg), to_pylot_transform(ego_vehicle.get_transform()), to_pylot_transform(pedestrian.get_transform()), BoundingBox(pedestrian.bounding_box), depth_camera_setup.get_unreal_transform(), depth_camera_setup.get_intrinsic(), resolution, 1.0, 3.0) if bbox is not None: detected_pedestrians.append(bbox) if visualize: draw_bounding_box(bbox, bb_surface) # We have drawn all the bounding boxes on the bb_surface, now put it on # the RGB image surface. if visualize: surface.blit(bb_surface, (0, 0)) pygame.display.flip() # Compute the mAP. print("We detected a total of {} pedestrians.".format(len(detected_pedestrians))) compute_and_log_map(detected_pedestrians, msg.timestamp, csv) # Move the ego_vehicle according to the given speed. ego_vehicle.set_velocity(carla.Vector3D(x=-speed)) ego_vehicle.get_world().tick()
def log_speed_limits(world): world_map = world.get_map() (_, traffic_lights, traffic_stops, speed_signs) = get_actors(world) transforms_of_interest = [] # Add transforms that are close to speed limit signs. for speed_sign in speed_signs: for offset in range(10, 25, 5): # Speed signs have different coordinate systems, hence # we need to offset y, instead of x. offset_loc = pylot.simulation.utils.Location(x=0, y=offset, z=0) offset_rot = pylot.simulation.utils.Rotation(pitch=0, yaw=0, roll=0) offset_trans = pylot.simulation.utils.Transform( offset_loc, offset_rot) transform = speed_sign.transform * offset_trans location = to_carla_location(transform.location) w = world_map.get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Driving) camera_transform = to_pylot_transform(w.transform) camera_transform.location.z += 2.0 transform = to_carla_transform(camera_transform) transforms_of_interest.append(transform) # Ensure all traffic lights are red. change_traffic_light_colors(world, carla.TrafficLightState.Red) world.tick() time.sleep(1) (_, traffic_lights, traffic_stops, speed_signs) = get_actors(world) for weather in find_weather_presets(): change_weather(world, weather) log_obstacles(world, transforms_of_interest, traffic_lights, carla.TrafficLightState.Red, speed_signs, traffic_stops, weather, world_map.name)
def on_depth_msg(carla_image): game_time = int(carla_image.timestamp * 1000) print("Received depth camera msg {}".format(game_time)) depth_camera_transform = to_pylot_transform(carla_image.transform) depth_msg = pylot.simulation.messages.DepthFrameMessage( depth_to_array(carla_image), depth_camera_transform, carla_image.fov, None) for (x, y) in pixels_to_check: print("{} Depth at pixel {}".format((x, y), depth_msg.frame[y][x])) pos3d_depth = get_3d_world_position_with_depth_map( x, y, depth_msg.frame, depth_msg.width, depth_msg.height, depth_msg.fov, depth_camera_transform) print("{} Computed using depth map {}".format((x, y), pos3d_depth)) depth_point_cloud = pylot.simulation.utils.depth_to_local_point_cloud( depth_msg.frame, depth_msg.width, depth_msg.height, depth_msg.fov, max_depth=1.0) # Transform the depth cloud to world coordinates. transform = camera_to_unreal_transform(depth_camera_transform) depth_point_cloud = transform.transform_points(depth_point_cloud) global depth_pc depth_pc = depth_point_cloud.tolist()
def compute_waypoints(self, source_loc, destination_loc): """ Computes waypoints between two locations. Assumes that the ego vehicle has the same orientation as the lane on whch it is on. Args: source_loc: Source world location. destination_loc: Destination world location. """ start_waypoint = self._map.get_waypoint( source_loc, project_to_road=True, lane_type=carla.LaneType.Driving) end_waypoint = self._map.get_waypoint( destination_loc, project_to_road=True, lane_type=carla.LaneType.Driving) assert start_waypoint and end_waypoint, 'Map could not find waypoints' route = self._grp.trace_route( start_waypoint.transform.location, end_waypoint.transform.location) # TODO(ionel): The planner returns several options in intersections. # We always take the first one, but this is not correct. return deque([to_pylot_transform(waypoint[0].transform) for waypoint in route])
def log_stop_signs(world): world_map = world.get_map() (_, traffic_lights, traffic_stops, speed_signs) = get_actors(world) transforms_of_interest = [] # Add transforms that are close to stop signs. for stop_sign in traffic_stops: for offset in range(10, 25, 5): offset_loc = pylot.simulation.utils.Location(x=-offset, y=0, z=0) offset_rot = pylot.simulation.utils.Rotation( pitch=0, yaw=0, roll=0) offset_trans = pylot.simulation.utils.Transform( offset_loc, offset_rot) transform = stop_sign.transform * offset_trans location = to_carla_location(transform.location) w = world_map.get_waypoint( location, project_to_road=True, lane_type=carla.LaneType.Driving) camera_transform = to_pylot_transform(w.transform) camera_transform.location.z += 2.0 transform = to_carla_transform(camera_transform) transforms_of_interest.append(transform) # Ensure all traffic lights are red. change_traffic_light_colors(world, carla.TrafficLightState.Red) world.tick() time.sleep(3) (_, traffic_lights, traffic_stops, speed_signs) = get_actors(world) log_obstacles(world, transforms_of_interest, traffic_lights, carla.TrafficLightState.Red, speed_signs, traffic_stops)
def send_can_bus_reading(self, data, timestamp, watermark_msg): # The can bus dict contains other fields as well, but we don't # curently use them. vehicle_transform = to_pylot_transform(data['transform']) forward_speed = data['speed'] can_bus = pylot.simulation.utils.CanBus(vehicle_transform, forward_speed) self._can_bus_stream.send(Message(can_bus, timestamp))
def __publish_hero_vehicle_data(self, timestamp, watermark_msg): vec_transform = to_pylot_transform( self._driving_vehicle.get_transform()) forward_speed = pylot.simulation.utils.get_speed( self._driving_vehicle.get_velocity()) can_bus = pylot.simulation.utils.CanBus(vec_transform, forward_speed) self.get_output_stream('can_bus').send(Message(can_bus, timestamp)) self.get_output_stream('can_bus').send(watermark_msg)
def log_bounding_boxes(carla_image, depth_frame, segmented_frame, traffic_lights, tl_color, speed_signs, stop_signs, weather, town): game_time = int(carla_image.timestamp * 1000) print("Processing game time {} in {} with weather {}".format( game_time, town, weather)) frame = bgra_to_bgr(to_bgra_array(carla_image)) # Copy the frame to ensure its on the heap. frame = copy.deepcopy(frame) transform = to_pylot_transform(carla_image.transform) _, world = get_world() town_name = world.get_map().name speed_limit_det_objs = [] if speed_signs: speed_limit_det_objs = pylot.simulation.utils.get_speed_limit_det_objs( speed_signs, transform, transform, depth_frame, FLAGS.frame_width, FLAGS.frame_height, FLAGS.camera_fov, segmented_frame) traffic_stop_det_objs = [] if stop_signs: traffic_stop_det_objs = pylot.simulation.utils.get_traffic_stop_det_objs( stop_signs, transform, depth_frame, FLAGS.frame_width, FLAGS.frame_height, FLAGS.camera_fov) traffic_light_det_objs = [] if traffic_lights: traffic_light_det_objs = get_traffic_light_objs( traffic_lights, transform, depth_frame, segmented_frame, FLAGS.frame_width, FLAGS.frame_height, tl_color, town_name) det_objs = (speed_limit_det_objs + traffic_stop_det_objs + traffic_light_det_objs) if FLAGS.visualize_bboxes: visualize_ground_bboxes('bboxes', game_time, frame, det_objs) # Log the frame. rgb_frame = bgr_to_rgb(frame) file_name = '{}signs-{}_{}_{}.png'.format(FLAGS.data_path, game_time, weather, town) rgb_img = Image.fromarray(np.uint8(rgb_frame)) rgb_img.save(file_name) if FLAGS.log_bbox_images: annotate_image_with_bboxes(game_time, frame, det_objs) rgb_frame = bgr_to_rgb(frame) file_name = '{}annotated-signs-{}_{}_{}.png'.format( FLAGS.data_path, game_time, weather, town) rgb_img = Image.fromarray(np.uint8(rgb_frame)) rgb_img.save(file_name) # Log the bounding boxes. bboxes = [det_obj.get_bbox_label() for det_obj in det_objs] file_name = '{}bboxes-{}_{}_{}.json'.format(FLAGS.data_path, game_time, weather, town) with open(file_name, 'w') as outfile: json.dump(bboxes, outfile)
def send_waypoints(self, timestamp): # Send once the global waypoints. if self._waypoints is None: self._waypoints = self._global_plan_world_coord data = [(to_pylot_transform(transform), road_option) for (transform, road_option) in self._waypoints] self._global_trajectory_stream.send(Message(data, timestamp)) #self._global_trajectory_stream.send(self._top_watermark) assert self._waypoints == self._global_plan_world_coord,\ 'Global plan has been updated.'
def convert_speed_limit_actors(speed_limit_actors): """ Converts a Carla speed limit actor into a Pylot speed limit object.""" speed_limits = [] for ts_actor in speed_limit_actors: transform = to_pylot_transform(ts_actor.get_transform()) speed_limit = int(ts_actor.type_id.split('.')[-1]) speed_sign = pylot.simulation.utils.SpeedLimitSign( transform, speed_limit) speed_limits.append(speed_sign) return speed_limits
def send_can_bus_reading(self, data, timestamp, watermark_msg): # The can bus dict contains other fields as well, but we don't # curently use them. vehicle_transform = to_pylot_transform(data['transform']) # TODO(ionel): Scenario runner computes speed differently from # the way we do it in the CARLA operator. This affects # agent stopping constants. Check! forward_speed = data['speed'] can_bus = pylot.simulation.utils.CanBus(vehicle_transform, forward_speed) self._can_bus_stream.send(Message(can_bus, timestamp))
def convert_pedestrian_actors(pedestrian_actors): """ Converts a Carla pedestrian actor into a Pylot pedestrian object.""" pedestrians = [] for ped_actor in pedestrian_actors: transform = to_pylot_transform(ped_actor.get_transform()) bounding_box = pylot.simulation.utils.BoundingBox( ped_actor.bounding_box) speed = pylot.simulation.utils.get_speed(ped_actor.get_velocity()) pedestrian = pylot.simulation.utils.Pedestrian(ped_actor.id, transform, bounding_box, speed) pedestrians.append(pedestrian) return pedestrians
def are_on_same_lane(self, location1, location2): """ Returns True if the two locations are on the same lane. Args: location1: Location in world coordinates. location1: Location in world coordinates. """ loc1 = to_carla_location(location1) waypoint1 = self._map.get_waypoint(loc1, project_to_road=False, lane_type=carla.LaneType.Driving) if not waypoint1: # First location is not on a drivable lane. return False loc2 = to_carla_location(location2) waypoint2 = self._map.get_waypoint(loc2, project_to_road=False, lane_type=carla.LaneType.Driving) if not waypoint2: # Second location is not on a drivable lane. return False w_t1 = to_pylot_transform(waypoint1.transform) w_t2 = to_pylot_transform(waypoint2.transform) self._logger.info('same_lane location1 {} to waypoint1 {}'.format( location1, w_t1.location)) self._logger.info('same_lane location2 {} to waypoint2 {}'.format( location2, w_t2.location)) if waypoint1.road_id == waypoint2.road_id: return waypoint1.lane_id == waypoint2.lane_id else: # Return False if we're in intersection and the other # obstacle isn't. if waypoint1.is_intersection and not waypoint2.is_intersection: return False if waypoint2.lane_type == carla.LaneType.Driving: # This may return True when the lane is different, but in # with a different road_id. # TODO(ionel): Figure out how lane id map across road id. return True return False
def convert_traffic_stop_actors(traffic_stop_actors): """ Converts a Carla traffic stop actor into a Pylot stop sign object.""" stop_signs = [] for ts_actor in traffic_stop_actors: transform = to_pylot_transform(ts_actor.get_transform()) world_trigger_volume = ts_actor.get_transform().transform( ts_actor.trigger_volume.location) bbox = pylot.simulation.utils.BoundingBox( carla.BoundingBox(world_trigger_volume, ts_actor.trigger_volume.extent)) stop_sign = pylot.simulation.utils.StopSign(transform, bbox) stop_signs.append(stop_sign) return stop_signs
def convert_vehicle_actors(vec_actors): """ Converts a Carla vehicle actor into a Pylot Vehicle object.""" vehicles = [] # TODO(ionel): Handle hero vehicle! for vec_actor in vec_actors: transform = to_pylot_transform(vec_actor.get_transform()) bounding_box = pylot.simulation.utils.BoundingBox( vec_actor.bounding_box) speed = pylot.simulation.utils.get_speed(vec_actor.get_velocity()) vehicle = pylot.simulation.utils.Vehicle(vec_actor.id, transform, bounding_box, speed) vehicles.append(vehicle) return vehicles
def __publish_hero_vehicle_data(self, timestamp, watermark_msg): vec_transform = to_pylot_transform( self._driving_vehicle.get_transform()) forward_speed = pylot.simulation.utils.get_speed( self._driving_vehicle.get_velocity()) can_bus = pylot.simulation.utils.CanBus(vec_transform, forward_speed) self.get_output_stream('can_bus').send(Message(can_bus, timestamp)) self.get_output_stream('can_bus').send(watermark_msg) # Set the world simulation view with respect to the vehicle. v_pose = self._driving_vehicle.get_transform() v_pose.location -= 10 * carla.Location(v_pose.get_forward_vector()) v_pose.location.z = 5 self._world.get_spectator().set_transform(v_pose)
def main(): world = setup_world() transform = get_spawnpoint(world, 0) print('Spawning at {}'.format(to_pylot_transform(transform))) try: camera = add_camera(world, transform, on_camera_msg) # Move the spectactor view to the camera position. world.get_spectator().set_transform(transform) world.tick() explore_api(world, transform) finally: # Destroy the actors. camera.destroy()
def convert_traffic_light_actor(tl_actor): transform = to_pylot_transform(tl_actor.get_transform()) tl_state = tl_actor.get_state() erdos_tl_state = None if tl_state == carla.TrafficLightState.Red: erdos_tl_state = TrafficLightColor.RED elif tl_state == carla.TrafficLightState.Yellow: erdos_tl_state = TrafficLightColor.YELLOW elif tl_state == carla.TrafficLightState.Green: erdos_tl_state = TrafficLightColor.GREEN else: erdos_tl_state = TrafficLightColor.OFF extent = pylot.simulation.utils.Extent(tl_actor.trigger_volume.extent.x, tl_actor.trigger_volume.extent.y, tl_actor.trigger_volume.extent.z) traffic_light = pylot.simulation.utils.TrafficLight( tl_actor.id, transform, erdos_tl_state, extent) return traffic_light
def on_lidar_msg(carla_pc): game_time = int(carla_pc.timestamp * 1000) print("Received lidar msg {}".format(game_time)) points = np.frombuffer(carla_pc.raw_data, dtype=np.dtype('f4')) points = copy.deepcopy(points) points = np.reshape(points, (int(points.shape[0] / 3), 3)) lidar_transform = to_pylot_transform(carla_pc.transform) # Transform lidar points from lidar coordinates to camera coordinates. points = lidar_point_cloud_to_camera_coordinates(points) for (x, y) in pixels_to_check: pos3d_pc = get_3d_world_position_with_point_cloud( x, y, points.tolist(), lidar_transform, 800, 600, 90.0) print("{} Computed using lidar {}".format((x, y), pos3d_pc)) global lidar_pc lidar_pc = points.tolist()
def convert_vehicle_actors(vec_actors): """ Converts a Carla vehicle object into a Pylot Vehicle object. Args: vec_actors: A list of Carla vehicle actors. Returns: A list of Pylot Vehicles. """ vehicles = [] for vec_actor in vec_actors: transform = to_pylot_transform(vec_actor.get_transform()) bounding_box = pylot.simulation.utils.BoundingBox( vec_actor.bounding_box) speed = pylot.simulation.utils.get_speed(vec_actor.get_velocity()) vehicle = pylot.simulation.utils.Vehicle(vec_actor.id, transform, bounding_box, speed) vehicles.append(vehicle) return vehicles
def main(args): """ The main function that orchestrates the setup of the world, connection to the scenario and the subsequent logging of the frames for computation of the mIoU. Args: args: The argparse.Namespace instance that is retrieved from parsing the arguments. """ # Setup the world. world = setup_world(args.host, args.port, args.delta) # Retrieve the ego-vehicle from the simulation. ego_vehicle = None while ego_vehicle is None: print("Waiting for the scenario to be ready ...") sleep(1) ego_vehicle = retrieve_actor(world, 'vehicle.*', 'hero') world.tick() # Transform of the cameras. camera_transform = carla.Transform(location=carla.Location(1.5, 0.0, 1.4), rotation=carla.Rotation(0, 0, 0)) # Connect the RGB camera to the vehicle. rgb_camera = spawn_camera('sensor.camera.rgb', camera_transform, ego_vehicle, *args.res.split('x')) # Connect the depth camera to the vehicle. depth_camera = spawn_camera('sensor.camera.depth', camera_transform, ego_vehicle, *args.res.split('x')) # Open the CSV file for writing. csv_file = open(args.output, 'w') csv_writer = csv.writer(csv_file) # Create the cleanup function. global CLEANUP_FUNCTION CLEANUP_FUNCTION = functools.partial(cleanup_function, world=world, cameras=[rgb_camera, depth_camera], csv_file=csv_file) # Create a PyGame surface for debugging purposes. width, height = map(int, args.res.split('x')) surface = None if args.visualize: surface = pygame.display.set_mode((width, height), pygame.HWSURFACE | pygame.DOUBLEBUF) # Register a callback function with the camera. rgb_camera.listen(process_rgb_images) depth_camera_setup = CameraSetup("depth_camera", 'sensor.camera.depth', width, height, to_pylot_transform(camera_transform)) depth_camera.listen( functools.partial(process_depth_images, depth_camera_setup=depth_camera_setup, ego_vehicle=ego_vehicle, speed=args.speed, csv=csv_writer, surface=surface, visualize=args.visualize)) try: # To keep the thread alive so that the images can be processed. while True: pass except KeyboardInterrupt: CLEANUP_FUNCTION() sys.exit(0)
def log_traffic_lights(world): world_map = world.get_map() (traffic_lights, _, traffic_stops, speed_signs) = get_actors(world) tl_colors = [ carla.TrafficLightState.Yellow, carla.TrafficLightState.Green, carla.TrafficLightState.Red ] for light in traffic_lights: print("Working for traffic light {}".format(light.id)) # For every traffic light, get the neighbouring lights except the one # directly opposite. group_lights = [] for n_light in light.get_group_traffic_lights(): if not check_lights_opposite(light, n_light): group_lights.append(convert_to_pylot_traffic_light(n_light)) transforms_of_interest = [] for offset in range(10, 40, 5): # Traffic lights have different coordinate systems, hence # we need to offset y, instead of x and add that to the trigger # volume location. offset_loc = pylot.simulation.utils.Location( x=light.trigger_volume.location.x, y=light.trigger_volume.location.y + offset, z=light.trigger_volume.location.z) offset_rot = pylot.simulation.utils.Rotation(pitch=0, yaw=0, roll=0) offset_trans = pylot.simulation.utils.Transform( offset_loc, offset_rot) # Transform the offset relative to the traffic light. transform = to_pylot_transform( light.get_transform()) * offset_trans location = to_carla_location(transform.location) # Get the waypoint nearest to the transform. w = world_map.get_waypoint(location, project_to_road=True, lane_type=carla.LaneType.Driving) w_rotation = w.transform.rotation camera_transform = to_pylot_transform(w.transform) camera_transform.location.z += 2.0 transform = to_carla_transform(camera_transform) transforms_of_interest.append(transform) # Get the right lanes. wp_right = w.get_right_lane() while wp_right and wp_right.lane_type == carla.LaneType.Driving \ and w_rotation == wp_right.transform.rotation: camera_transform = to_pylot_transform(wp_right.transform) camera_transform.location.z += 2.0 transform = to_carla_transform(camera_transform) transforms_of_interest.append(transform) wp_right = wp_right.get_right_lane() # Get the left lanes. wp_left = w.get_left_lane() while wp_left and wp_left.lane_type == carla.LaneType.Driving and \ w_rotation == wp_left.transform.rotation: camera_transform = to_pylot_transform(wp_left.transform) camera_transform.location.z += 2.0 transform = to_carla_transform(camera_transform) transforms_of_interest.append(transform) wp_left = wp_left.get_left_lane() print("The total number of transforms were: {}".format( len(transforms_of_interest))) for tl_color in tl_colors: change_traffic_light_colors(world, tl_color) world.tick() time.sleep(3) log_obstacles(world, transforms_of_interest, group_lights, tl_color, speed_signs, traffic_stops)