def execute(self): # TODO(ionel): We should send the failure message on a failed stream, # and get a reply back so that we know the node is not processing or # issuing any new messages. Otherwise, we run into the risk of having # inconsistent state because we can't guarantees in-order delivery # across operators. # Send failure message. if self._pre_failure_time_elapse_s is not None: time.sleep(self._pre_failure_time_elapse_s) # Fail primary and notify ingress, egress and consumers fail_replica_num = 0 fail_msg = Message( (flux_utils.FluxControllerCommand.FAIL, fail_replica_num), Timestamp(coordinates=[0])) self.get_output_stream('controller_stream').send(fail_msg) print("Control send failure message to primary") if self._failure_duration_s is not None: # TODO(yika): recovery does not work yet # Recover failed replica time.sleep(self._failure_duration_s) fail_msg = Message((flux_utils.FluxControllerCommand.RECOVER, fail_replica_num), Timestamp(coordinates=[0])) self.get_output_stream('controller_stream').send(fail_msg) self.spin()
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 publish_msg(self): msg = Message(self._cnt, Timestamp(coordinates=[self._cnt])) self.get_output_stream('data_stream').send(msg) tuple_msg = Message((self._cnt, 'counter {}'.format(self._cnt)), Timestamp(coordinates=[self._cnt])) self.get_output_stream('tuple_data_stream').send(tuple_msg) list_msg = Message([self._cnt, self._cnt + 1], Timestamp(coordinates=[self._cnt])) self.get_output_stream('list_data_stream').send(list_msg) self._cnt += 1
def on_notification(self, msg): self._logger.info("Timestamps {} {} {} {}".format( self._obstacles[0].timestamp, self._traffic_lights[0].timestamp, self._depth_imgs[0].timestamp, self._world_transform[0].timestamp)) world_transform = self._world_transform[0].data self._world_transform = self._world_transform[1:] depth_img = self._depth_imgs[0].data self._depth_imgs = self._depth_imgs[1:] traffic_lights = self.__transform_tl_output(depth_img, world_transform) self._traffic_lights = self._traffic_lights[1:] (pedestrians, vehicles) = self.__transform_detector_output(depth_img, world_transform) self._obstacles = self._obstacles[1:] speed_factor, state = self.__stop_for_agents(self._wp_angle, self._wp_vector, vehicles, pedestrians, traffic_lights) control = self.get_control(self._wp_angle, self._wp_angle_speed, speed_factor, self._vehicle_speed * 3.6) output_msg = Message(control, Timestamp(coordinates=[0])) self.get_output_stream('action_stream').send(output_msg)
def execute(self): while self._seq_num < self._num_messages: output_msg = Message("data-" + str(self._seq_num), Timestamp(coordinates=[self._seq_num])) self.get_output_stream('input_stream').send(output_msg) self._seq_num += 1 time.sleep(self._time_gap)
def on_msg_best_sync_data_at_freq(self): """Publishes best synced data received since last invocation.""" self._epoch += 1 timestamp = Timestamp(coordinates=[self._epoch - 1]) msg = Message(self._best_sync[self._epoch - 1], timestamp) self.get_output_stream('sync_stream').send(msg) del self._best_sync[self._epoch - 1]
def publish_msg(self): if self.idx % 2 == 0: time.sleep(0.02) data = 'data %d' % self.idx output_msg = Message(data, Timestamp(coordinates=[0])) self.get_output_stream('pub_out').send(output_msg) self.idx += 1
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 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 calculate_directions(self): pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime) # Send value 0-5 for direction (e.g., 0 left, 1 right, ...). direction = np.random.randint(0, 6) output_msg = Message(direction, Timestamp(coordinates=[self._cnt])) self.get_output_stream('directions').send(output_msg) self._cnt += 1
def on_loop_complete(self, msg): """Publishes best synced data received while the loop was running.""" self._epoch += 1 timestamp = Timestamp(coordinates=[self._epoch - 1]) msg = Message(self._best_sync[self._epoch - 1], timestamp) self.get_output_stream('sync_stream').send(msg) del self._best_sync[self._epoch - 1]
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 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 predict(self): predicted_locs = self._objs + self._objs + self._objs pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime) output_msg = Message(predicted_locs, Timestamp(coordinates=[self._cnt])) self.get_output_stream('prediction_stream').send(output_msg) self._objs = [] self._cnt += 1
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 publish_imu_data(self): roll = random.random() pitch = random.random() yaw = random.random() output_msg = Message((roll, pitch, yaw), Timestamp(coordinates=[self._cnt])) self.get_output_stream('imu').send(output_msg) self._cnt += 1
def localize(self): pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime) # TODO(ionel): Check how synchronized the data is. # TODO(ionel): Interact with the mapping operator. location = (random.uniform(0, 180), random.uniform(0, 180)) output_msg = Message(location, Timestamp(coordinates=[self._cnt])) self.get_output_stream('location').send(output_msg) self._cnt += 1
def publish_msg(self): self._cnt += 1 # XXX(ionel): Hack! We send more messages because the first several # messages are lost in ROS because subscribers may start after # publishers already publish several messages. if self._cnt <= self._num_msgs + 10000: output_msg = Message(self._cnt, Timestamp(coordinates=[self._cnt])) self.get_output_streams('data_stream').send(output_msg)
def on_reply(self, msg): self._cnt += 1 if self._cnt == self._num_iterations: duration = time.time() - self._start_time print('Completed {} iterations in {}'.format(self._cnt, duration)) else: output_msg = Message(self._cnt, Timestamp(coordinates=[0])) self.get_output_stream('loop_ingest').send(output_msg)
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 publish_frame(self): # TODO(ionel): Should send RGBD instead of RGB. cv_image = self.create_image(1280, 960) image = self._bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") image.header.seq = self._cnt output_msg = Message(image, Timestamp(coordinates=[self._cnt])) output_name = '{}_output'.format(self.name) self.get_output_stream(output_name).send(output_msg) self._cnt += 1
def __init__( self, timestamp: PyTimestamp, config: "erdos.OperatorConfig", write_stream: WriteStream, ): self.timestamp = Timestamp(_py_timestamp=timestamp) self.config = config self.write_stream = write_stream
def publish_frame(self): """Publish frames out camera stream every 100ms.""" cv_image = self.read_image(self._cnt % NUM_FRAMES) image = self._bridge.cv2_to_imgmsg(cv_image, encoding="bgr8") image.header.seq = self._cnt output_msg = Message(image, Timestamp(coordinates=[self._cnt])) output_name = '{}_output'.format(self.name) self.get_output_stream(output_name).send(output_msg) self._cnt += 1
def execute_sum(self, message): """ Sum all the numbers in this window and send out the aggregate. """ batch_number = message.timestamp.coordinates[0] window_data = self.windows.pop(batch_number, None) #print("Received a watermark for the timestamp: {}".format(batch_number)) #print("The sum of the window {} is {}".format( # window_data, sum(window_data))) output_msg = Message(sum(window_data), Timestamp(coordinates=[batch_number])) self.get_output_stream("sum_out").send(output_msg)
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 __init__( self, timestamp: PyTimestamp, config: "erdos.OperatorConfig", left_write_stream: WriteStream, right_write_stream: WriteStream, ): self.timestamp = Timestamp(_py_timestamp=timestamp) self.config = config self.left_write_stream = left_write_stream self.right_write_stream = right_write_stream
def publish_inputs(self): while True: msg_delay_time = random.random() time.sleep(msg_delay_time) # data = 'message {} from {}'.format(self._epoch, self.name) data = time.time() * 1000.0 msg = Message(data, Timestamp(coordinates=[self._epoch])) print('Publish {}'.format(msg)) self.get_output_stream('{}_output'.format(self.name)).send(msg) time.sleep(1.0 - msg_delay_time) self._epoch += 1
def publish_frame(self): """Publish mock camera frames.""" cv_image = self.read_image(self._cnt) self._image = self.bridge.cv2_to_imgmsg(cv_image, encoding='bgr8') self._image.header.seq = self._cnt timestamp = Timestamp(coordinates=[self._cnt, self._cnt]) output_msg = Message(self._image, timestamp) self.get_output_stream('{}_output'.format(self.name)).send(output_msg) self.get_output_stream('{}_output'.format(self.name)).send( WatermarkMessage(timestamp)) self._cnt += 1
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 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)