def send_pose_msg(self, speed_data, imu_data, gnss_data, timestamp):
forward_speed = speed_data['speed']
# vehicle_transform = pylot.utils.Transform.from_carla_transform(
# speed_data['transform'])
latitude = gnss_data[0]
longitude = gnss_data[1]
altitude = gnss_data[2]
location = pylot.utils.Location.from_gps(latitude, longitude, altitude)
if np.isnan(imu_data[6]):
yaw = self._last_yaw
else:
compass = np.degrees(imu_data[6])
if compass < 270:
yaw = compass - 90
else:
yaw = compass - 450
self._last_yaw = yaw
vehicle_transform = pylot.utils.Transform(
location, pylot.utils.Rotation(yaw=yaw))
velocity_vector = pylot.utils.Vector3D(forward_speed * np.cos(yaw),
forward_speed * np.sin(yaw), 0)
current_pose = pylot.utils.Pose(vehicle_transform, forward_speed,
velocity_vector,
timestamp.coordinates[0])
self._logger.debug("@{}: Predicted pose: {}".format(
timestamp, current_pose))
self._pose_stream.send(erdos.Message(timestamp, current_pose))
self._pose_stream.send(erdos.WatermarkMessage(timestamp))
def callback(self, msg, write_stream):
self.msgs.append(msg)
if len(self.msgs) == self.window_size:
msg = erdos.Message(msg.timestamp, self.msgs)
write_stream.send(msg)
self.msgs = []
def main(argv):
(obstacles_stream, traffic_lights_stream, obstacles_tracking_stream,
open_drive_stream, global_trajectory_stream) = create_data_flow()
# Run the data-flow.
erdos.run_async()
top_timestamp = erdos.Timestamp(coordinates=[sys.maxsize])
open_drive_stream.send(erdos.WatermarkMessage(top_timestamp))
waypoints = [[waypoint] for waypoint in read_waypoints()]
global_trajectory_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]), waypoints))
global_trajectory_stream.send(erdos.WatermarkMessage(top_timestamp))
time_to_sleep = 1.0 / FLAGS.sensor_frequency
count = 0
while True:
timestamp = erdos.Timestamp(coordinates=[count])
if not FLAGS.obstacle_detection:
obstacles_stream.send(ObstaclesMessage(timestamp, []))
obstacles_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.traffic_light_detection:
traffic_lights_stream.send(TrafficLightsMessage(timestamp, []))
traffic_lights_stream.send(erdos.WatermarkMessage(timestamp))
if not FLAGS.obstacle_tracking:
obstacles_tracking_stream.send(
ObstacleTrajectoriesMessage(timestamp, []))
obstacles_tracking_stream.send(erdos.WatermarkMessage(timestamp))
count += 1
# NOTE: We should offset sleep time by the time it takes to send the
# messages.
time.sleep(time_to_sleep)
def on_pose_update(self, msg, time_to_decision_stream):
self._logger.debug('@{}: {} received pose message'.format(
msg.timestamp, self.config.name))
ttd = TimeToDecisionOperator.time_to_decision(msg.data.transform,
msg.data.forward_speed,
None)
time_to_decision_stream.send(erdos.Message(msg.timestamp, ttd))
def run(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.")
timestamp = erdos.Timestamp(is_top=True)
vehicle_id_msg = erdos.Message(timestamp, self._driving_vehicle.id)
self._vehicle_id_stream.send(vehicle_id_msg)
self._vehicle_id_stream.send(erdos.WatermarkMessage(timestamp))
self._world.on_tick(self.on_world_tick)
def run_visualizer_control_loop(control_display_stream):
"""Runs a pygame loop that waits for user commands.
The user commands are send on the control_display_stream
to control the pygame visualization window.
"""
import erdos
import pygame
clock = pygame.time.Clock()
from pygame.locals import K_n
while True:
clock.tick_busy_loop(60)
events = pygame.event.get()
for event in events:
if event.type == pygame.KEYUP:
if event.key == K_n:
control_display_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
event.key))
elif event.type == pygame.QUIT:
raise KeyboardInterrupt
elif event.type == pygame.KEYDOWN:
if (event.key == pygame.K_c
and pygame.key.get_mods() & pygame.KMOD_CTRL):
raise KeyboardInterrupt
def add(msg):
"""Mapping Function passed into MapOp,
returns a new Message that sums the data of each message in msg.data."""
total = 0
for i in msg.data:
total += i.data
return erdos.Message(msg.timestamp, total)
def run(self):
count = 0
payload = '0' * self.size
while True:
msg = erdos.Message(erdos.Timestamp(coordinates=[count]), payload)
self.write_stream.send(msg)
#print("SendOp: sending {msg}".format(msg=msg))
count += 1
def run(self, write_stream: WriteStream):
count = 0
while True:
msg = erdos.Message(erdos.Timestamp(coordinates=[count]), count)
print(f"SendOp sending {msg}")
write_stream.send(msg)
count += 1
time.sleep(1)
def send_hd_map_msg(self, data, timestamp):
# Sending once opendrive data
if self._open_drive_data is None:
self._open_drive_data = data['opendrive']
self._open_drive_stream.send(
erdos.Message(timestamp, self._open_drive_data))
self._open_drive_stream.send(
erdos.WatermarkMessage(
erdos.Timestamp(coordinates=[sys.maxsize])))
def __send_world_data(self):
""" Sends ego vehicle id, open drive and trajectory messages."""
# Send the id of the ego vehicle. This id is used by the driver
# operators to get a handle to the ego vehicle, which they use to
# attach sensors.
self.vehicle_id_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
self._ego_vehicle.id))
self.vehicle_id_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
# Send open drive string.
self.open_drive_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
self._world.get_map().to_opendrive()))
top_watermark = erdos.WatermarkMessage(erdos.Timestamp(is_top=True))
self.open_drive_stream.send(top_watermark)
self.global_trajectory_stream.send(top_watermark)
def __publish_hero_vehicle_data(self, timestamp, watermark_msg):
vec_transform = pylot.utils.Transform.from_carla_transform(
self._driving_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_carla_vector(
self._driving_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector)
self._pose_stream.send(erdos.Message(timestamp, pose))
self._pose_stream.send(watermark_msg)
def run(self):
count = 0
while True:
msg = erdos.Message(erdos.Timestamp(coordinates=[count]), count)
print("SendOp: sending {msg}".format(msg=msg))
self.write_stream.send(msg)
count += 1
time.sleep(1)
def send_initial_pose_msg(self, timestamp):
if not self._sent_initial_pose:
self._sent_initial_pose = True
initial_transform = self._global_plan_world_coord[0][0]
initial_pose = pylot.utils.Pose(
pylot.utils.Transform.from_carla_transform(initial_transform),
0, pylot.utils.Vector3D(), timestamp.coordinates[0])
self._route_stream.send(erdos.Message(timestamp, initial_pose))
self._route_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def send_perfect_pose_msg(self, timestamp):
vec_transform = pylot.utils.Transform.from_carla_transform(
self._ego_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_carla_vector(
self._ego_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector,
timestamp.coordinates[0])
self._pose_stream.send(erdos.Message(timestamp, pose))
self._pose_stream.send(erdos.WatermarkMessage(timestamp))
def __send_hero_vehicle_data(self, stream, timestamp, watermark_msg):
vec_transform = pylot.utils.Transform.from_carla_transform(
self._ego_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_carla_vector(
self._ego_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
print("{} Forward speed is {}".format(timestamp, forward_speed))
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector)
stream.send(erdos.Message(timestamp, pose))
stream.send(erdos.WatermarkMessage(timestamp))
def callback(self, msg, write_stream):
self.msgs.append(msg)
if len(self.msgs) >= self.window_size:
msg = erdos.Message(msg.timestamp, self.msgs[0:self.window_size])
write_stream.send(msg)
self.msgs = self.msgs[self.offset:]
self.count += 1
def _send_world_messages(self):
""" Sends initial open drive and trajectory messages."""
# Send open drive string.
top_timestamp = erdos.Timestamp(coordinates=[sys.maxsize])
self.open_drive_stream.send(
erdos.Message(top_timestamp,
self._world.get_map().to_opendrive()))
top_watermark = erdos.WatermarkMessage(top_timestamp)
self.open_drive_stream.send(top_watermark)
self.global_trajectory_stream.send(top_watermark)
def send_opendrive_map_msg(self, data, timestamp):
# Sending once opendrive data
if self._open_drive_data is None:
self._open_drive_data = data['opendrive']
self._open_drive_stream.send(
erdos.Message(timestamp, self._open_drive_data))
self._open_drive_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
else:
self._logger.warning(
'Agent did not sent open drive data for {}'.format(timestamp))
def send_can_bus_msg(self, data, timestamp):
# The can bus dict contains other fields as well, but we don't use
# them yet.
vehicle_transform = pylot.utils.Transform.from_carla_transform(
data['transform'])
forward_speed = data['speed']
self._can_bus_stream.send(
erdos.Message(timestamp,
pylot.utils.CanBus(vehicle_transform,
forward_speed)))
self._can_bus_stream.send(erdos.WatermarkMessage(timestamp))
def send_waypoints_msg(self, timestamp):
# Send once the global waypoints.
if self._waypoints is None:
# Gets global waypoints from the agent.
self._waypoints = self._global_plan_world_coord
data = [(pylot.utils.Transform.from_carla_transform(transform),
road_option)
for (transform, road_option) in self._waypoints]
self._global_trajectory_stream.send(erdos.Message(timestamp, data))
self._global_trajectory_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
def on_msg_camera_stream(self, msg, detected_lanes_stream):
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self._name))
start_time = time.time()
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
image_np = msg.frame.frame
# TODO(ionel): Implement lane detection.
edges = self.apply_canny(image_np)
lines_edges = self.apply_hough(image_np, edges)
kernel_size = 3
grad_x = self.apply_sobel(image_np,
orient='x',
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
grad_y = self.apply_sobel(image_np,
orient='y',
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
mag_binary = self.magnitude_threshold(image_np,
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=255)
dir_binary = self.direction_threshold(image_np,
sobel_kernel=kernel_size,
thresh_min=0,
thresh_max=np.pi / 2)
s_binary = self.extract_s_channel(image_np)
# Select the pixels where both x and y gradients meet the threshold
# criteria, or the gradient magnitude and direction are both with
# the threshold values.
combined = np.zeros_like(dir_binary)
combined[((grad_x == 1) & (grad_y == 1)) | ((mag_binary == 1) &
(dir_binary == 1))] = 1
combined_binary = np.zeros_like(grad_x)
combined_binary[(s_binary == 1) | (grad_x == 1)] = 1
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self._name, msg.timestamp,
runtime))
if self._flags.visualize_lane_detection:
frame = CameraFrame(lines_edges, 'BGR', msg.frame.camera_setup)
frame.visualize(self._name, msg.timestamp)
detected_lanes_stream.send(erdos.Message(msg.timestamp, image_np))
def on_data(self, context: OneInOneOutContext, data: int):
print("LoopOp: received {data}".format(data=data))
time.sleep(1)
# Update data and timestamp.
data += 1
coordinates = list(context.timestamp.coordinates)
coordinates[0] += 1
timestamp = erdos.Timestamp(coordinates=coordinates)
message = erdos.Message(timestamp, data)
print("LoopOp: sending {message}".format(message=message))
context.write_stream.send(message)
def on_msg_camera_stream(self, msg, detected_lanes_stream):
"""Invoked whenever a frame message is received on the stream.
Args:
msg: A :py:class:`~pylot.perception.messages.FrameMessage`.
detected_lanes_stream (:py:class:`erdos.WriteStream`): Stream on
which the operator sends
:py:class:`~pylot.perception.messages.DetectedLaneMessage`
messages.
"""
self._logger.debug('@{}: {} received message'.format(
msg.timestamp, self.config.name))
assert msg.frame.encoding == 'BGR', 'Expects BGR frames'
# Make a copy of the image coming into the operator.
image = np.copy(msg.frame.as_numpy_array())
# Get the dimensions of the image.
x_lim, y_lim = image.shape[1], image.shape[0]
# Convert to grayscale.
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Apply gaussian blur.
image = cv2.GaussianBlur(image, (self._kernel_size, self._kernel_size),
0)
# Apply the Canny Edge Detector.
image = cv2.Canny(image, 30, 60)
# Define a region of interest.
points = np.array(
[[
(0, y_lim), # Bottom left corner.
(0, y_lim - 60),
(x_lim // 2 - 20, y_lim // 2),
(x_lim // 2 + 20, y_lim // 2),
(x_lim, y_lim - 60),
(x_lim, y_lim), # Bottom right corner.
]],
dtype=np.int32)
image = self._region_of_interest(image, points)
# Hough lines.
image = self._draw_lines(image)
if self._flags.visualize_lane_detection:
final_img = np.copy(msg.frame.as_numpy_array())
final_img = cv2.addWeighted(final_img, 0.8, image, 1.0, 0.0)
frame = CameraFrame(final_img, 'BGR', msg.frame.camera_setup)
frame.visualize(self.config.name,
msg.timestamp,
pygame_display=pylot.utils.PYGAME_DISPLAY)
detected_lanes_stream.send(erdos.Message(msg.timestamp, image))
def run(self):
count = 0
while True:
timestamp = erdos.Timestamp(coordinates=[count])
msg = erdos.Message(timestamp, count)
print("{name}: sending {msg}".format(name=self.config.name,
msg=msg))
self.write_stream.send(msg)
count += 1
time.sleep(1 / self.frequency)
def __send_hero_vehicle_data(self, stream: WriteStream,
timestamp: Timestamp):
vec_transform = pylot.utils.Transform.from_simulator_transform(
self._ego_vehicle.get_transform())
velocity_vector = pylot.utils.Vector3D.from_simulator_vector(
self._ego_vehicle.get_velocity())
forward_speed = velocity_vector.magnitude()
pose = pylot.utils.Pose(vec_transform, forward_speed, velocity_vector,
timestamp.coordinates[0])
stream.send(erdos.Message(timestamp, pose))
stream.send(erdos.WatermarkMessage(timestamp))
def main():
# Create the first dataflow.
ingest_stream = erdos.IngestStream()
(map_stream, ) = erdos.connect(map.Map,
erdos.OperatorConfig(name="Double"),
[ingest_stream],
function=double)
extract_stream = erdos.ExtractStream(map_stream)
node_handle = erdos.run_async()
for t in range(5):
send_msg = erdos.Message(erdos.Timestamp(coordinates=[t]), t)
ingest_stream.send(send_msg)
print("IngestStream: sent {send_msg}".format(send_msg=send_msg))
recv_msg = extract_stream.read()
print("ExtractStream: received {recv_msg}".format(recv_msg=recv_msg))
time.sleep(1)
node_handle.shutdown()
erdos.reset()
# Create a new dataflow.
ingest_stream = erdos.IngestStream()
(map_stream, ) = erdos.connect(map.Map,
erdos.OperatorConfig(name="Square"),
[ingest_stream],
function=square)
extract_stream = erdos.ExtractStream(map_stream)
node_handle = erdos.run_async()
for t in range(5):
send_msg = erdos.Message(erdos.Timestamp(coordinates=[t]), t)
ingest_stream.send(send_msg)
print("IngestStream: sent {send_msg}".format(send_msg=send_msg))
recv_msg = extract_stream.read()
print("ExtractStream: received {recv_msg}".format(recv_msg=recv_msg))
time.sleep(1)
node_handle.shutdown()
def process_visualization_events(control_display_stream):
if pylot.flags.must_visualize():
import pygame
from pygame.locals import K_n
events = pygame.event.get()
for event in events:
if event.type == pygame.KEYUP:
# Change the visualization view when n is pressed.
if event.key == K_n:
control_display_stream.send(
erdos.Message(erdos.Timestamp(coordinates=[0]),
event.key))
def on_ground_segmented_frame(self, msg, iou_stream):
assert len(msg.timestamp.coordinates) == 1
start_time = time.time()
# We don't fully transform it to cityscapes palette to avoid
# introducing extra latency.
frame = msg.frame
sim_time = msg.timestamp[0]
if len(self._ground_frames) > 0:
# Pop the oldest frame if it's older than the max latency
# we're interested in.
if (msg.timestamp.coordinates[0] - self._ground_frames[0][0] >
self._flags.decay_max_latency):
self._ground_frames.popleft()
cur_time = time_epoch_ms()
for timestamp, ground_frame in self._ground_frames:
(mean_iou, class_iou) = \
ground_frame.compute_semantic_iou_using_masks(frame)
time_diff = msg.timestamp.coordinates[0] - timestamp
self._logger.info(
'Segmentation ground latency {} ; mean IoU {}'.format(
time_diff, mean_iou))
self._csv_logger.info('{},{},{},mIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff, mean_iou))
iou_stream.send(
erdos.Message(msg.timestamp, (time_diff, mean_iou)))
person_key = 4
if person_key in class_iou:
self._logger.info(
'Segmentation ground latency {} ; person IoU {}'.
format(time_diff, class_iou[person_key]))
self._csv_logger.info(
'{},{},{},personIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff,
class_iou[person_key]))
vehicle_key = 10
if vehicle_key in class_iou:
self._logger.info(
'Segmentation ground latency {} ; vehicle IoU {}'.
format(time_diff, class_iou[vehicle_key]))
self._csv_logger.info(
'{},{},{},vehicleIoU,{},{:.4f}'.format(
cur_time, sim_time, self.config.name, time_diff,
class_iou[vehicle_key]))
# Append the processed image to the buffer.
self._ground_frames.append((msg.timestamp.coordinates[0], frame))
runtime = (time.time() - start_time) * 1000
self._logger.info(
'Segmentation eval ground runtime {}'.format(runtime))
def run(self):
count = 0
while True:
timestamp = erdos.Timestamp(coordinates=[count])
msg = erdos.Message(timestamp, count)
print("SendOp: sending {msg}".format(msg=msg))
self.write_stream.send(msg)
if count % 3 == 2:
print("sendOp: sending watermark")
self.write_stream.send(erdos.WatermarkMessage(timestamp))
count += 1
time.sleep(1)
