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)
