def on_watermark(self, timestamp, ground_tracking_stream):
self._logger.debug('@{}: received watermark'.format(timestamp))
obstacles_msg = self._obstacles_raw_msgs.popleft()
can_bus_msg = self._can_bus_msgs.popleft()
# Use the most recent can_bus message to convert the past frames
# of vehicles and people to our current perspective.
can_bus_transform = can_bus_msg.data.transform
obstacle_trajectories = []
# Only consider obstacles which still exist at the most recent
# timestamp.
for obstacle in obstacles_msg.obstacles:
self._obstacles[obstacle.id].append(obstacle)
cur_obstacle_trajectory = []
# Iterate through past frames of this obstacle.
for past_obstacle_loc in self._obstacles[obstacle.id]:
# Get the location of the center of the obstacle's bounding
# box, in relation to the CanBus measurement.
v_transform = past_obstacle_loc.transform * \
past_obstacle_loc.bounding_box.transform
new_location = can_bus_transform.inverse_transform_points(
[v_transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(location=new_location,
rotation=pylot.utils.Rotation()))
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
cur_obstacle_trajectory))
output_msg = ObstacleTrajectoriesMessage(timestamp,
obstacle_trajectories)
ground_tracking_stream.send(output_msg)
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_watermark(self, timestamp, tracked_obstacles_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
tracked_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
return
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._pose_msgs.popleft().data.transform
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
# Ignore obstacles that are far away.
if (vehicle_transform.location.distance(
obstacle.transform.location) >
self._flags.dynamic_obstacle_distance_threshold):
continue
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
# The trajectory is relative to the current location.
obstacle_trajectories.append(
ObstacleTrajectory(obstacle, cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
tracked_obstacles_stream.send(erdos.WatermarkMessage(timestamp))
self._log_obstacles(timestamp, obstacles_with_location)
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
def on_watermark(self, timestamp, tracked_obstacles_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._pose_msgs.popleft().data.transform
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
for obstacle in obstacles_with_location:
obstacle_location = obstacle.transform.location
x = obstacle_location.x
y = obstacle_location.y
z = obstacle_location.z
self._csv_logger.debug('{},{},obstacle,{},{}'.format(
pylot.utils.time_epoch_ms(), timestamp.coordinates[0],
"[{} {}]".format(obstacle.id, obstacle.label),
"[{:.4f} {:.4f} {:.4f}]".format(x, y, z)))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
def on_watermark(self, timestamp, tracked_obstacles_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._pose_msgs.popleft().data.transform
frame_msg = self._frame_msgs.popleft()
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
# Transform obstacle location from global world coordinates to
# ego-centric coordinates.
cur_obstacle_trajectory = []
for obstacle in self._obstacle_history[obstacle.id]:
new_location = \
vehicle_transform.inverse_transform_locations(
[obstacle.transform.location])[0]
cur_obstacle_trajectory.append(
pylot.utils.Transform(new_location,
pylot.utils.Rotation()))
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
if self._flags.visualize_obstacles_with_distance:
frame_msg.frame.annotate_with_bounding_boxes(
timestamp, obstacles_with_location, vehicle_transform)
frame_msg.frame.visualize(
self.config.name, pygame_display=pylot.utils.PYGAME_DISPLAY)
def on_watermark(self, timestamp, tracked_obstacles_stream):
"""Invoked when all input streams have received a watermark.
Args:
timestamp (:py:class:`erdos.timestamp.Timestamp`): The timestamp of
the watermark.
"""
self._logger.debug('@{}: received watermark'.format(timestamp))
obstacles_msg = self._obstacles_msgs.popleft()
depth_msg = self._depth_msgs.popleft()
vehicle_transform = self._can_bus_msgs.popleft().data.transform
frame_msg = self._frame_msgs.popleft()
obstacles_with_location = get_obstacle_locations(
obstacles_msg.obstacles, depth_msg, vehicle_transform,
self._camera_setup, self._logger)
ids_cur_timestamp = []
obstacle_trajectories = []
for obstacle in obstacles_with_location:
ids_cur_timestamp.append(obstacle.id)
self._obstacle_history[obstacle.id].append(obstacle)
cur_obstacle_trajectory = [
obstacle.transform
for obstacle in self._obstacle_history[obstacle.id]
]
obstacle_trajectories.append(
ObstacleTrajectory(obstacle.label, obstacle.id,
obstacle.bounding_box,
cur_obstacle_trajectory))
tracked_obstacles_stream.send(
ObstacleTrajectoriesMessage(timestamp, obstacle_trajectories))
self._timestamp_history.append(timestamp)
self._timestamp_to_id[timestamp] = ids_cur_timestamp
if len(self._timestamp_history) >= self._flags.tracking_num_steps:
gc_timestamp = self._timestamp_history.popleft()
for obstacle_id in self._timestamp_to_id[gc_timestamp]:
self._obstacle_history[obstacle_id].popleft()
if len(self._obstacle_history[obstacle_id]) == 0:
del self._obstacle_history[obstacle_id]
del self._timestamp_to_id[gc_timestamp]
if self._flags.visualize_obstacles_with_distance:
frame_msg.frame.annotate_with_bounding_boxes(
timestamp, obstacles_with_location, vehicle_transform)
frame_msg.frame.visualize(self.config.name)
def on_watermark(self, timestamp: Timestamp,
ground_tracking_stream: WriteStream):
self._logger.debug('@{}: received watermark'.format(timestamp))
if timestamp.is_top:
return
obstacles_msg = self._obstacles_raw_msgs.popleft()
pose_msg = self._pose_msgs.popleft()
# Use the most recent pose message to convert the past frames
# of vehicles and people to our current perspective.
pose_transform = pose_msg.data.transform
obstacle_trajectories = []
# Only consider obstacles which still exist at the most recent
# timestamp.
for obstacle in obstacles_msg.obstacles:
if obstacle.id == self._vehicle_id and not \
self._flags.prediction_ego_agent:
# If we are not performing ego-agent prediction, do not
# track the ego-vehicle.
continue
if (pose_transform.location.distance(obstacle.transform.location) >
self._flags.dynamic_obstacle_distance_threshold):
# Ignore the obstacle if it is too far away.
continue
self._obstacles[obstacle.id].append(obstacle)
cur_obstacle_trajectory = []
# Iterate through past frames of this obstacle.
for past_obstacle_loc in self._obstacles[obstacle.id]:
# Get the transform of the center of the obstacle's bounding
# box, in relation to the Pose measurement.
v_transform = past_obstacle_loc.transform * \
past_obstacle_loc.bounding_box.transform
new_transform = (pose_transform.inverse_transform() *
v_transform)
cur_obstacle_trajectory.append(new_transform)
obstacle_trajectories.append(
ObstacleTrajectory(obstacle, cur_obstacle_trajectory))
output_msg = ObstacleTrajectoriesMessage(timestamp,
obstacle_trajectories)
ground_tracking_stream.send(output_msg)
def main(argv):
(obstacles_stream, traffic_lights_stream, obstacles_tracking_stream,
open_drive_stream, global_trajectory_stream, control_display_stream,
streams_to_send_top_on) = create_data_flow()
# Run the data-flow.
node_handle = erdos.run_async()
signal.signal(signal.SIGINT, shutdown)
# Send waypoints.
waypoints = Waypoints.read_from_csv_file(FLAGS.waypoints_csv_file,
FLAGS.target_speed)
global_trajectory_stream.send(
WaypointsMessage(
erdos.Timestamp(coordinates=[0]), waypoints,
pylot.planning.utils.BehaviorPlannerState.FOLLOW_WAYPOINTS))
# Send top watermark on all streams that require it.
top_msg = erdos.WatermarkMessage(erdos.Timestamp(is_top=True))
open_drive_stream.send(top_msg)
global_trajectory_stream.send(top_msg)
for stream in streams_to_send_top_on:
stream.send(top_msg)
time_to_sleep = 1.0 / FLAGS.sensor_frequency
count = 0
try:
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
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:
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
# NOTE: We should offset sleep time by the time it takes to send
# the messages.
time.sleep(time_to_sleep)
except KeyboardInterrupt:
node_handle.shutdown()
if pylot.flags.must_visualize():
import pygame
pygame.quit()