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))