def publish_numbers(self): """ Sends an increasing count of numbers with breaks at pre-defined steps.""" output_msg = Message(self.counter, Timestamp(coordinates=[self.batch_number])) if not self.send_batch(self.batch_number): # If this batch_number does not need to be sent as a batch, then # send the individual messages and if the batch is complete, send a # watermark. self.get_output_stream(self.output_stream_name).send(output_msg) if self.counter % self.batch_size == 0: # The batch has completed, send the watermark. watermark_msg = WatermarkMessage( Timestamp(coordinates=[self.batch_number])) self.batch_number += 1 self.get_output_stream( self.output_stream_name).send(watermark_msg) else: # This batch number needs to be batched, add it to the window and # then check if the batch_size number of messages exist in the # window. If yes, send everything including the watermark message. self.window.append(output_msg) if self.counter % self.batch_size == 0: # The batch has completed, send everything including the # watermark. for output_msg in self.window: self.get_output_stream( self.output_stream_name).send(output_msg) watermark_msg = WatermarkMessage( Timestamp(coordinates=[self.batch_number])) self.batch_number += 1 self.get_output_stream( self.output_stream_name).send(watermark_msg) self.window = [] self.counter += 1
def on_completion_msg(self, msg): """Invokes corresponding callback for stream stream_name.""" self._op.log_event(time.time(), msg.timestamp, 'receive watermark {}'.format(msg.stream_name)) # Ensure that the watermark is monotonically increasing. high_watermark = self._op._stream_to_high_watermark[msg.stream_name] if not high_watermark: # The first watermark, just set the dictionary with the value. self._op._stream_to_high_watermark[ msg.stream_name] = msg.timestamp else: if high_watermark >= msg.timestamp: raise Exception( "The watermark received in the msg {} is not " "higher than the watermark previously received " "on the same stream: {}".format(msg, high_watermark)) else: self._op._stream_to_high_watermark[msg.stream_name] = \ msg.timestamp # Now check if all other streams have a higher or equal watermark. # If yes, flow this watermark. If not, return from this function # Also, maintain the lowest watermark observed. low_watermark = msg.timestamp for stream, watermark in self._op._stream_to_high_watermark.items(): # TODO(yika): big HACK on ignoring watermark sent on stream # with label 'no_watermark' = true if (stream not in self._op._stream_ignore_watermarks and stream != msg.stream_name): if not watermark or watermark < msg.timestamp: return if low_watermark > watermark: low_watermark = watermark new_msg = WatermarkMessage(low_watermark) new_msg.stream_uid = msg.stream_uid # Checkpoint. # Note: For correctness reasons, we can only flow watermarks after # we checkpoint. if (self._op._checkpoint_enable and self._op.checkpoint_condition(msg.timestamp)): self._op._checkpoint(msg.timestamp) # Call the required callbacks. for cb in self._completion_callbacks.get(new_msg.stream_uid, []): cb(new_msg) # Finished calling the required callbacks. Send the watermark forward # to the dependent operators. # TODO (sukritk) :: Same issue as erdos/ros/ros_input_data_stream.py # TODO (sukritk) FIX (Ray Issue #4463): Remove when Ray issue is fixed. if not self._completion_callbacks.get(new_msg.stream_uid): watermark_msg = WatermarkMessage(msg.timestamp, msg.stream_name) for output_stream in self._op.output_streams.values(): output_stream.send(watermark_msg)
def process_images(self, carla_image): """ Invoked when an image is received from the simulator. Args: carla_image: a carla.Image. """ # Ensure that the code executes serially with self._lock: game_time = int(carla_image.timestamp * 1000) timestamp = Timestamp(coordinates=[game_time]) watermark_msg = WatermarkMessage(timestamp) msg = None if self._camera_setup.camera_type == 'sensor.camera.rgb': msg = pylot.simulation.messages.FrameMessage( pylot.utils.bgra_to_bgr(to_bgra_array(carla_image)), timestamp) elif self._camera_setup.camera_type == 'sensor.camera.depth': # Include the transform relative to the vehicle. # Carla carla_image.transform returns the world transform, but # we do not use it directly. msg = pylot.simulation.messages.DepthFrameMessage( depth_to_array(carla_image), self._camera_setup.get_transform(), carla_image.fov, timestamp) elif self._camera_setup.camera_type == 'sensor.camera.semantic_segmentation': frame = labels_to_array(carla_image) msg = SegmentedFrameMessage(frame, 0, timestamp) # Send the message containing the frame. self.get_output_stream(self._camera_setup.name).send(msg) # Note: The operator is set not to automatically propagate # watermark messages received on input streams. Thus, we can # issue watermarks only after the Carla callback is invoked. self.get_output_stream(self._camera_setup.name).send(watermark_msg)
def process_point_clouds(self, carla_pc): """ Invoked when a pointcloud is received from the simulator. Args: carla_pc: a carla.SensorData object. """ # Ensure that the code executes serially with self._lock: game_time = int(carla_pc.timestamp * 1000) timestamp = Timestamp(coordinates=[game_time]) watermark_msg = WatermarkMessage(timestamp) # Transform the raw_data into a point cloud. 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)) # Include the transform relative to the vehicle. # Carla carla_pc.transform returns the world transform, but # we do not use it directly. msg = PointCloudMessage(points, self._lidar_setup.get_transform(), timestamp) self.get_output_stream(self._lidar_setup.name).send(msg) # Note: The operator is set not to automatically propagate # watermark messages received on input streams. Thus, we can # issue watermarks only after the Carla callback is invoked. self.get_output_stream(self._lidar_setup.name).send(watermark_msg)
def execute(self): # Connect to CARLA and retrieve the world running. self._client, self._world = get_world(self._flags.carla_host, self._flags.carla_port, self._flags.carla_timeout) if self._client is None or self._world is None: raise ValueError('There was an issue connecting to the simulator.') # Replayer time factor is only available in > 0.9.5. # self._client.set_replayer_time_factor(0.1) print( self._client.replay_file(self._flags.carla_replay_file, self._flags.carla_replay_start_time, self._flags.carla_replay_duration, self._flags.carla_replay_id)) # Sleep a bit to allow the server to start the replay. time.sleep(1) self._driving_vehicle = self._world.get_actors().find( self._flags.carla_replay_id) if self._driving_vehicle is None: raise ValueError("There was an issue finding the vehicle.") # TODO(ionel): We do not currently have a top message. timestamp = Timestamp(coordinates=[sys.maxint]) vehicle_id_msg = Message(self._driving_vehicle.id, timestamp) self.get_output_stream('vehicle_id_stream').send(vehicle_id_msg) self.get_output_stream('vehicle_id_stream').send( WatermarkMessage(timestamp)) self._world.on_tick(self.on_world_tick) self.spin()
def publish_world_data(self): read_start_time = time.time() measurements, sensor_data = self.client.read_data() measure_time = time.time() self._logger.info( 'Got readings for game time {} and platform time {}'.format( measurements.game_timestamp, measurements.platform_timestamp)) timestamp = Timestamp(coordinates=[measurements.game_timestamp]) watermark = WatermarkMessage(timestamp) # Send player data on data streams. self.__send_player_data(measurements.player_measurements, timestamp, watermark) # Extract agent data from measurements. agents = self.__get_ground_agents(measurements) # Send agent data on data streams. self.__send_ground_agent_data(agents, timestamp, watermark) # Send sensor data on data stream. self.__send_sensor_data(sensor_data, timestamp, watermark) # Get Autopilot control data. if self._auto_pilot: self.control = measurements.player_measurements.autopilot_control end_time = time.time() measurement_runtime = (measure_time - read_start_time) * 1000 total_runtime = (end_time - read_start_time) * 1000 self._logger.info('Carla measurement time {}; total time {}'.format( measurement_runtime, total_runtime)) self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name, measurement_runtime, total_runtime))
def execute(self): while self._seq_num < self._num_messages: # Send msg and watermark timestamp = Timestamp(coordinates=[self._seq_num]) output_msg = Message(self._seq_num, timestamp) watermark = WatermarkMessage(timestamp) pub = self.get_output_stream('input_stream') pub.send(output_msg) pub.send(watermark) self._seq_num += 1 time.sleep(self._time_gap)
def on_completion_msg(self, msg): stream_name = self.timestamp_to_stream_name_map.get(msg.timestamp) if stream_name: # The stream name exists. We should check if the watermark is from # the stream. If yes, release, otherwise let it be. if stream_name == msg.stream_name: # TODO (sukritk) FIX (Ray #4463): Remove when Ray issue fixed. watermark_msg = WatermarkMessage(msg.timestamp, msg.stream_name) self.get_output_stream( self.output_stream_name).send(watermark_msg)
def run_step(self, input_data, timestamp): with self._lock: self._control = None self._control_timestamp = None global GAME_TIME GAME_TIME += 1 print("Current game time {}".format(GAME_TIME)) erdos_timestamp = Timestamp(coordinates=[GAME_TIME]) watermark = WatermarkMessage(erdos_timestamp) if self.track != Track.ALL_SENSORS_HDMAP_WAYPOINTS: if not self._sent_open_drive_data: self._sent_open_drive_data = True #self._open_drive_stream.send(self._top_watermark) self.send_waypoints(erdos_timestamp) for key, val in input_data.items(): #print("{} {} {}".format(key, val[0], type(val[1]))) if key in self._camera_names: # Send camera frames. self._camera_streams[key].send( pylot.simulation.messages.FrameMessage( bgra_to_bgr(val[1]), erdos_timestamp)) #self._camera_streams[key].send(watermark) elif key == 'can_bus': self.send_can_bus_reading(val[1], erdos_timestamp, watermark) elif key == 'GPS': gps = pylot.simulation.utils.LocationGeo( val[1][0], val[1][1], val[1][2]) elif key == 'hdmap': self.send_hd_map_reading(val[1], erdos_timestamp) elif key == 'LIDAR': self.send_lidar_reading(val[1], erdos_timestamp, watermark) # Wait until the control is set. output_control = None with self._lock: # Ensure that control is not reset by another run_step invocation # from another thread when a new scenario is loaded. while (self._control_timestamp is None or self._control_timestamp < erdos_timestamp): time.sleep(0.01) # Create output message. We create the VehicleControl while we # held the lock to ensure that control does not get changed. output_control = carla.VehicleControl() output_control.throttle = self._control.throttle output_control.brake = self._control.brake output_control.steer = self._control.steer output_control.reverse = self._control.reverse output_control.hand_brake = self._control.hand_brake output_control.manual_gear_shift = False return output_control
def _parse_message(internal_msg): """Creates a Message from an internal stream's response. Args: internal_msg (PyMessage): The internal message to parse. """ if internal_msg.is_timestamped_data(): return pickle.loads(internal_msg.data) if internal_msg.is_watermark(): return WatermarkMessage( Timestamp(_py_timestamp=internal_msg.timestamp)) raise Exception("Unable to parse message")
def publish_world_data(self, msg): """ Callback function that gets called when the world is ticked. This function sends a WatermarkMessage to the downstream operators as a signal that they need to release data to the rest of the pipeline. Args: msg: Data recieved from the simulation at a tick. """ game_time = int(msg.elapsed_seconds * 1000) self._logger.info('The world is at the timestamp {}'.format(game_time)) # Create a timestamp and send a WatermarkMessage on the output stream. timestamp = Timestamp(coordinates=[game_time]) watermark_msg = WatermarkMessage(timestamp) self.__publish_hero_vehicle_data(timestamp, watermark_msg) self.__publish_ground_actors_data(timestamp, watermark_msg)
def on_world_tick(self, msg): """ Callback function that gets called when the world is ticked. This function sends a WatermarkMessage to the downstream operators as a signal that they need to release data to the rest of the pipeline. Args: msg: Data recieved from the simulation at a tick. """ with self._lock: game_time = int(msg.elapsed_seconds * 1000) timestamp = Timestamp(coordinates=[game_time]) watermark_msg = WatermarkMessage(timestamp) self.__publish_hero_vehicle_data(timestamp, watermark_msg) self.__publish_ground_actors_data(timestamp, watermark_msg) # XXX(ionel): Hack! Sleep a bit to not overlead the subscribers. time.sleep(0.2)
def publish_numbers(self): """ Sends an increasing count of numbers to the downstream operators. """ output_msg = Message(self.counter, Timestamp(coordinates=[self.batch_number])) self.get_output_stream("integer_out").send(output_msg) # Decide if the watermark needs to be sent. if self.counter % self.batch_size == 0: # The batch has completed. We need to send a watermark now. watermark_msg = WatermarkMessage( Timestamp(coordinates=[self.batch_number])) self.batch_number += 1 self.get_output_stream("integer_out").send(watermark_msg) # Update the counters. self.counter += 1
def _parse_message(internal_msg: PyMessage) -> Message[T] | WatermarkMessage: """Creates a Message from an internal stream's response. Args: internal_msg: The internal message to parse. """ if internal_msg.is_timestamped_data(): assert ( internal_msg.data is not None), "Timestamped data message should always have data." timestamp = Timestamp(_py_timestamp=internal_msg.timestamp) data = pickle.loads(internal_msg.data) return Message(timestamp, data) if internal_msg.is_watermark(): return WatermarkMessage( Timestamp(_py_timestamp=internal_msg.timestamp)) raise Exception("Unable to parse message")
def execute(self): # Register a callback function and a function that ticks the world. # TODO(ionel): We do not currently have a top message. timestamp = Timestamp(coordinates=[sys.maxint]) vehicle_id_msg = Message(self._driving_vehicle.id, timestamp) self.get_output_stream('vehicle_id_stream').send(vehicle_id_msg) self.get_output_stream('vehicle_id_stream').send( WatermarkMessage(timestamp)) # XXX(ionel): Hack to fix a race condition. Driver operators # register a carla listen callback only after they've received # the vehicle id value. We miss frames if we tick before # they register a listener. Thus, we sleep here a bit to # give them sufficient time to register a callback. time.sleep(5) self._world.on_tick(self.publish_world_data) self._tick_simulator() self.spin()
def publish_watermarks(self): """ Sends watermarks to the downstream operators. """ # First, send the watermark on one stream. watermark_msg = WatermarkMessage(Timestamp(coordinates=[self.counter])) self.get_output_stream("output_1").send(watermark_msg) # Send messages on the other stream. message_1 = Message(self.counter, Timestamp(coordinates=[self.counter])) message_2 = Message(self.counter + 1, Timestamp(coordinates=[self.counter])) self.get_output_stream("output_2").send(message_1) self.get_output_stream("output_2").send(message_2) # Now, send the watermark on the second stream. self.get_output_stream("output_2").send(watermark_msg) self.counter += 1
def on_notification(self, msg): # Pop the oldest message from each buffer. with self._lock: if not self.synchronize_msg_buffers( msg.timestamp, [self._depth_imgs, self._bgr_imgs, self._segmented_imgs, self._can_bus_msgs, self._pedestrians, self._vehicles, self._traffic_lights, self._speed_limit_signs, self._stop_signs]): return depth_msg = self._depth_imgs.popleft() bgr_msg = self._bgr_imgs.popleft() segmented_msg = self._segmented_imgs.popleft() can_bus_msg = self._can_bus_msgs.popleft() pedestrians_msg = self._pedestrians.popleft() vehicles_msg = self._vehicles.popleft() traffic_light_msg = self._traffic_lights.popleft() speed_limit_signs_msg = self._speed_limit_signs.popleft() stop_signs_msg = self._stop_signs.popleft() self._logger.info('Timestamps {} {} {} {} {} {}'.format( depth_msg.timestamp, bgr_msg.timestamp, segmented_msg.timestamp, can_bus_msg.timestamp, pedestrians_msg.timestamp, vehicles_msg.timestamp, traffic_light_msg.timestamp)) # The popper messages should have the same timestamp. assert (depth_msg.timestamp == bgr_msg.timestamp == segmented_msg.timestamp == can_bus_msg.timestamp == pedestrians_msg.timestamp == vehicles_msg.timestamp == traffic_light_msg.timestamp) self._frame_cnt += 1 if (hasattr(self._flags, 'log_every_nth_frame') and self._frame_cnt % self._flags.log_every_nth_frame != 0): # There's no point to run the perfect detector if collecting # data, and only logging every nth frame. output_msg = DetectorMessage([], 0, msg.timestamp) self.get_output_stream(self._output_stream_name).send(output_msg) self.get_output_stream(self._output_stream_name)\ .send(WatermarkMessage(msg.timestamp)) return depth_array = depth_msg.frame segmented_image = segmented_msg.frame vehicle_transform = can_bus_msg.data.transform det_ped = self.__get_pedestrians(pedestrians_msg.pedestrians, vehicle_transform, depth_array, segmented_image) det_vec = self.__get_vehicles(vehicles_msg.vehicles, vehicle_transform, depth_array, segmented_image) det_traffic_lights = pylot.simulation.utils.get_traffic_light_det_objs( traffic_light_msg.traffic_lights, vehicle_transform * depth_msg.transform, depth_msg.frame, depth_msg.width, depth_msg.height, self._town_name, depth_msg.fov) det_speed_limits = pylot.simulation.utils.get_speed_limit_det_objs( speed_limit_signs_msg.speed_signs, vehicle_transform, vehicle_transform * depth_msg.transform, depth_msg.frame, depth_msg.width, depth_msg.height, depth_msg.fov, segmented_msg.frame) det_stop_signs = pylot.simulation.utils.get_traffic_stop_det_objs( stop_signs_msg.stop_signs, vehicle_transform * depth_msg.transform, depth_msg.frame, depth_msg.width, depth_msg.height, depth_msg.fov) det_objs = (det_ped + det_vec + det_traffic_lights + det_speed_limits + det_stop_signs) # Send the detected obstacles. output_msg = DetectorMessage(det_objs, 0, msg.timestamp) self.get_output_stream(self._output_stream_name).send(output_msg) # Send watermark on the output stream because operators do not # automatically forward watermarks when they've registed an # on completion callback. self.get_output_stream(self._output_stream_name)\ .send(WatermarkMessage(msg.timestamp)) if (self._flags.visualize_ground_obstacles or self._flags.log_detector_output): annotate_image_with_bboxes( bgr_msg.timestamp, bgr_msg.frame, det_objs) if self._flags.visualize_ground_obstacles: visualize_image(self.name, bgr_msg.frame) if self._flags.log_detector_output: save_image(pylot.utils.bgr_to_rgb(bgr_msg.frame), bgr_msg.timestamp, self._flags.data_path, 'perfect-detector')
def _on_msg(self, msg): #data = msg if self.data_type else pickle.loads(msg.data) msg = pickle.loads(msg.data) self.op.log_event(time.time(), msg.timestamp, 'receive {}'.format(self.name)) if isinstance(msg, WatermarkMessage): if msg.stream_name in self.op._stream_ignore_watermarks: # TODO(yika): big HACK on ignoring watermark sent on stream # with label 'no_watermark' = true return # Ensure that the watermark is monotonically increasing. high_watermark = self.op._stream_to_high_watermark[msg.stream_name] if not high_watermark: # The first watermark, just set the dictionary with the value. self.op._stream_to_high_watermark[ msg.stream_name] = msg.timestamp else: if high_watermark >= msg.timestamp: raise Exception( "The watermark received in the msg {} at operator {} " "is not higher than the watermark previously received " "on the same stream: {}".format( msg, self.op.name, high_watermark)) else: self.op._stream_to_high_watermark[msg.stream_name] = \ msg.timestamp # Now check if all other streams have a higher or equal watermark. # If yes, flow this watermark. If not, return from this function # Also, maintain the lowest watermark observed. low_watermark = msg.timestamp for stream, watermark in self.op._stream_to_high_watermark.items(): # TODO(yika): big HACK on ignoring watermark sent on stream # with label 'no_watermark' = true if (stream not in self.op._stream_ignore_watermarks and stream != msg.stream_name): # Low watermark does not change if there exists another # stream without a watermark or with a watermark smaller # than the previous watermark on the current stream. if (not watermark or (high_watermark is not None and watermark <= high_watermark)): return if low_watermark > watermark: low_watermark = watermark msg = WatermarkMessage(low_watermark) # Checkpoint. # Note: For correctness reasons, we can only flow watermarks after # we checkpoint. if (self.op._checkpoint_enable and self.op.checkpoint_condition(msg.timestamp)): self.op._checkpoint(msg.timestamp) # Call the required callbacks. for on_watermark_callback in self.completion_callbacks: on_watermark_callback(self.op, msg) # If no completion callbacks are found, let the watermarks flow # automatically. If there is a completion callback, let the # developer flow the watermarks. # TODO (sukritk) :: Either define an API to know when the system # has to flow watermarks, or figure out if the developer has already # sent a watermark for a timestamp and don't send duplicates. if (len(self.completion_callbacks) == 0 and not self.op._no_watermark_passthrough): for output_stream in self.op.output_streams.values(): output_stream.send(msg) else: for on_msg_callback in self.callbacks: on_msg_callback(self.op, msg)
def _parse_message(internal_msg): """Creates a Message from an internal stream's response.""" time_coordinates, serialized_data = internal_msg if serialized_data is None: return WatermarkMessage(Timestamp(coordinates=time_coordinates)) return pickle.loads(serialized_data)
def _flow_watermark_callback(timestamp, *write_streams): """Flows a watermark to all write streams.""" for write_stream in write_streams: write_stream.send(WatermarkMessage(timestamp))
def on_notification(self, msg): # Pop the oldest message from each buffer. with self._lock: if not self.synchronize_msg_buffers( msg.timestamp, [self._vehicles_raw_msgs, self._pedestrians_raw_msgs, self._can_bus_msgs]): return vehicles_msg = self._vehicles_raw_msgs.popleft() pedestrians_msg = self._pedestrians_raw_msgs.popleft() can_bus_msg = self._can_bus_msgs.popleft() self._logger.info('Timestamps {} {} {}'.format( vehicles_msg.timestamp, pedestrians_msg.timestamp, can_bus_msg.timestamp)) # The popper messages should have the same timestamp. assert (vehicles_msg.timestamp == pedestrians_msg.timestamp == can_bus_msg.timestamp) self._frame_cnt += 1 # Use the most recent can_bus message to convert the past frames # of vehicles and pedestrians to our current perspective. inv_can_bus_transform = can_bus_msg.data.transform.inverse_transform() vehicle_trajectories = [] # Only consider vehicles which still exist at the most recent # timestamp. for vehicle in vehicles_msg.vehicles: self._vehicles[vehicle.id].append(vehicle) cur_vehicle_trajectory = [] # Iterate through past frames of this vehicle. for past_vehicle_loc in self._vehicles[vehicle.id]: # Get the location of the center of the vehicle's bounding box, # in relation to the CanBus measurement. new_transform = inv_can_bus_transform * \ past_vehicle_loc.transform * \ past_vehicle_loc.bounding_box.transform cur_vehicle_trajectory.append(new_transform.location) vehicle_trajectories.append(ObjTrajectory('vehicle', vehicle.id, cur_vehicle_trajectory)) pedestrian_trajectories = [] # Only consider pedestrians which still exist at the most recent # timestamp. for ped in pedestrians_msg.pedestrians: self._pedestrians[ped.id].append(ped) cur_ped_trajectory = [] # Iterate through past frames for this pedestrian. for past_ped_loc in self._pedestrians[ped.id]: # Get the location of the center of the pedestrian's bounding # box, in relation to the CanBus measurement. new_transform = inv_can_bus_transform * \ past_ped_loc.transform * \ past_ped_loc.bounding_box.transform cur_ped_trajectory.append(new_transform.location) pedestrian_trajectories.append(ObjTrajectory('pedestrian', ped.id, cur_ped_trajectory)) output_msg = ObjTrajectoriesMessage( vehicle_trajectories + pedestrian_trajectories, msg.timestamp) self.get_output_stream(self._output_stream_name).send(output_msg) self.get_output_stream(self._output_stream_name)\ .send(WatermarkMessage(msg.timestamp))
def setup(self, path_to_conf_file): flags.FLAGS([__file__, '--flagfile={}'.format(path_to_conf_file)]) if FLAGS.track == 1: self.track = Track.ALL_SENSORS elif FLAGS.track == 2: self.track = Track.CAMERAS elif FLAGS.track == 3: self.track = Track.ALL_SENSORS_HDMAP_WAYPOINTS else: print('Unexpected track {}'.format(FLAGS.track)) (bgr_camera_setup, all_camera_setups) = self.__create_camera_setups() self._camera_setups = all_camera_setups self._lidar_transform = Transform(Location(1.5, 0.0, 1.40), Rotation(0, 0, 0)) self._camera_names = {CENTER_CAMERA_NAME} if FLAGS.depth_estimation or self.track == Track.CAMERAS: self._camera_names.add(LEFT_CAMERA_NAME) self._camera_names.add(RIGHT_CAMERA_NAME) self._camera_streams = {} self._lock = threading.Lock() # Planning related attributes self._waypoints = None self._sent_open_drive_data = False self._open_drive_data = None # TODO(ionel): We should have a top watermark. global TOP_TIME self._top_watermark = WatermarkMessage( Timestamp(coordinates=[TOP_TIME])) TOP_TIME += 1 # Set up graph self.graph = erdos.graph.get_current_graph() # The publishers must be re-created every time the Agent object # is constructed. self._input_streams = self.__create_input_streams() global ROS_NODE_INITIALIZED # We only initialize the data-flow once. On the first run. # We do not re-initialize it before each run because ROS does not # support several init_node calls from the same process or from # a process started with Python multiprocess. if not ROS_NODE_INITIALIZED: self.__initialize_data_flow(self._input_streams, bgr_camera_setup) ROS_NODE_INITIALIZED = True # Initialize the driver script as a ROS node so that we can receive # data back from the data-flow. rospy.init_node('erdos_driver', anonymous=True) # Subscribe to the control stream. self._subscriber = rospy.Subscriber( 'default/pylot_agent/control_stream', String, callback=self.__on_control_msg, queue_size=None) # Setup all the input streams. for input_stream in self._input_streams: input_stream.setup() time.sleep(5)
def read_carla_data(self): read_start_time = time.time() measurements, sensor_data = self.client.read_data() measure_time = time.time() self._logger.info( 'Got readings for game time {} and platform time {}'.format( measurements.game_timestamp, measurements.platform_timestamp)) # Send measurements player_measurements = measurements.player_measurements vehicle_pos = ((player_measurements.transform.location.x, player_measurements.transform.location.y, player_measurements.transform.location.z), (player_measurements.transform.orientation.x, player_measurements.transform.orientation.y, player_measurements.transform.orientation.z)) world_transform = Transform(player_measurements.transform) timestamp = Timestamp( coordinates=[measurements.game_timestamp, self.message_num]) self.message_num += 1 ray.register_custom_serializer(Message, use_pickle=True) ray.register_custom_serializer(WatermarkMessage, use_pickle=True) watermark = WatermarkMessage(timestamp) self.get_output_stream('world_transform').send( Message(world_transform, timestamp)) self.get_output_stream('world_transform').send(watermark) self.get_output_stream('vehicle_pos').send( Message(vehicle_pos, timestamp)) self.get_output_stream('vehicle_pos').send(watermark) acceleration = (player_measurements.acceleration.x, player_measurements.acceleration.y, player_measurements.acceleration.z) self.get_output_stream('acceleration').send( Message(acceleration, timestamp)) self.get_output_stream('acceleration').send(watermark) self.get_output_stream('forward_speed').send( Message(player_measurements.forward_speed, timestamp)) self.get_output_stream('forward_speed').send(watermark) self.get_output_stream('vehicle_collisions').send( Message(player_measurements.collision_vehicles, timestamp)) self.get_output_stream('vehicle_collisions').send(watermark) self.get_output_stream('pedestrian_collisions').send( Message(player_measurements.collision_pedestrians, timestamp)) self.get_output_stream('pedestrian_collisions').send(watermark) self.get_output_stream('other_collisions').send( Message(player_measurements.collision_other, timestamp)) self.get_output_stream('other_collisions').send(watermark) self.get_output_stream('other_lane').send( Message(player_measurements.intersection_otherlane, timestamp)) self.get_output_stream('other_lane').send(watermark) self.get_output_stream('offroad').send( Message(player_measurements.intersection_offroad, timestamp)) self.get_output_stream('offroad').send(watermark) vehicles = [] pedestrians = [] traffic_lights = [] speed_limit_signs = [] for agent in measurements.non_player_agents: if agent.HasField('vehicle'): pos = messages.Transform(agent.vehicle.transform) bb = messages.BoundingBox(agent.vehicle.bounding_box) forward_speed = agent.vehicle.forward_speed vehicle = messages.Vehicle(pos, bb, forward_speed) vehicles.append(vehicle) elif agent.HasField('pedestrian'): if not self.agent_id_map.get(agent.id): self.pedestrian_count += 1 self.agent_id_map[agent.id] = self.pedestrian_count pedestrian_index = self.agent_id_map[agent.id] pos = messages.Transform(agent.pedestrian.transform) bb = messages.BoundingBox(agent.pedestrian.bounding_box) forward_speed = agent.pedestrian.forward_speed pedestrian = messages.Pedestrian(pedestrian_index, pos, bb, forward_speed) pedestrians.append(pedestrian) elif agent.HasField('traffic_light'): transform = messages.Transform(agent.traffic_light.transform) traffic_light = messages.TrafficLight( transform, agent.traffic_light.state) traffic_lights.append(traffic_light) elif agent.HasField('speed_limit_sign'): transform = messages.Transform( agent.speed_limit_sign.transform) speed_sign = messages.SpeedLimitSign( transform, agent.speed_limit_sign.speed_limit) speed_limit_signs.append(speed_sign) vehicles_msg = Message(vehicles, timestamp) self.get_output_stream('vehicles').send(vehicles_msg) self.get_output_stream('vehicles').send(watermark) pedestrians_msg = Message(pedestrians, timestamp) self.get_output_stream('pedestrians').send(pedestrians_msg) self.get_output_stream('pedestrians').send(watermark) traffic_lights_msg = Message(traffic_lights, timestamp) self.get_output_stream('traffic_lights').send(traffic_lights_msg) self.get_output_stream('traffic_lights').send(watermark) traffic_sings_msg = Message(speed_limit_signs, timestamp) self.get_output_stream('traffic_signs').send(traffic_sings_msg) self.get_output_stream('traffic_signs').send(watermark) # Send sensor data for name, measurement in sensor_data.items(): data_stream = self.get_output_stream(name) if data_stream.get_label('camera_type') == 'SceneFinal': # Transform the Carla RGB images to BGR. data_stream.send( Message( pylot_utils.bgra_to_bgr(to_bgra_array(measurement)), timestamp)) else: data_stream.send(Message(measurement, timestamp)) data_stream.send(watermark) self.client.send_control(**self.control) end_time = time.time() measurement_runtime = (measure_time - read_start_time) * 1000 total_runtime = (end_time - read_start_time) * 1000 self._logger.info('Carla measurement time {}; total time {}'.format( measurement_runtime, total_runtime)) self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name, measurement_runtime, total_runtime))